long: copy also long preamble for channel estimation

lib/ofdm_sync_long.cc

@@ -27,29 +27,29 @@ using namespace gr::ieee802_11;
 using namespace std;
 
 
+bool compare_abs(const std::pair<gr_complex, int>& first, const std::pair<gr_complex, int>& second) {
+	return abs(get<0>(first)) > abs(get<0>(second));
+}
+
 class ofdm_sync_long_impl : public ofdm_sync_long {
 
 public:
 ofdm_sync_long_impl(unsigned int sync_length, bool log, bool debug) : block("ofdm_sync_long",
 		gr::io_signature::make2(2, 2, sizeof(gr_complex), sizeof(gr_complex)),
 		gr::io_signature::make(1, 1, sizeof(gr_complex))),
+		d_fir(gr::filter::kernel::fir_filter_ccc(1, LONG)),
 		d_log(log),
 		d_debug(debug),
 		d_offset(0),
-		d_freq_est(0),
 		d_state(SYNC),
 		SYNC_LENGTH(sync_length) {
 
 	set_tag_propagation_policy(block::TPP_DONT);
-	d_fir = new gr::filter::kernel::fir_filter_ccc(1, LONG);
 	d_correlation = gr::fft::malloc_complex(8192);
-	d_freq_est_buf = static_cast<gr_complex*>(std::malloc(sync_length * sizeof(gr_complex)));
 }
 
 ~ofdm_sync_long_impl(){
-	delete d_fir;
 	gr::fft::free(d_correlation);
-	free(d_freq_est_buf);
 }
 
 int general_work (int noutput, gr_vector_int& ninput_items,
@@ -64,19 +64,21 @@ int general_work (int noutput, gr_vector_int& ninput_items,
 			ninput_items[1] << "  noutput " << noutput <<
 			"   state " << d_state << std::endl;
 
-	int ninput = std::min(ninput_items[0], ninput_items[1]);
+	int ninput = std::min(std::min(ninput_items[0], ninput_items[1]), 8192);
 
 	const unsigned int nread = nitems_read(0);
 	get_tags_in_range(d_tags, 0, nread, nread + ninput);
 	if (d_tags.size()) {
 		std::sort(d_tags.begin(), d_tags.end(), gr::tag_t::offset_compare);
 
-		const gr::tag_t &tag = d_tags.front();
+		const uint64_t offset = d_tags.front().offset;
-		const uint64_t offset = tag.offset;
 
 		if(offset > nread) {
 			ninput = offset - nread;
 		} else {
+			if(d_offset && (d_state == SYNC)) {
+				throw std::runtime_error("wtf");
+			}
 			if(d_state == COPY) {
 				d_state = RESET;
 			}
@@ -90,26 +92,20 @@ int general_work (int noutput, gr_vector_int& ninput_items,
 	switch(d_state) {
 
 	case SYNC:
-		d_fir->filterN(d_correlation, in, std::min(SYNC_LENGTH, std::max(ninput - 63, 0)));
+		d_fir.filterN(d_correlation, in, std::min(SYNC_LENGTH, std::max(ninput - 63, 0)));
 
 		while(i + 63 < ninput) {
 
-			d_freq_est += in[i] * conj(in[i + 16]);
+			d_cor.push_back(pair<gr_complex, int>(d_correlation[i], d_offset));
-			d_freq_est_buf[d_offset] = d_freq_est;
-
-			d_cor.push_back(pair<double, int>(abs(d_correlation[i]), d_offset));
 
 			i++;
 			d_offset++;
 
 			if(d_offset == SYNC_LENGTH) {
 				search_frame_start();
-				d_freq_est = d_freq_est_buf[std::max<int>(0, d_frame_start - 160 - 17)];
 				d_offset = 0;
 				d_state = COPY;
 
-				mylog(boost::format("frame at %1% - freq_est (20M): %2%") % d_frame_start
-						% ((arg(d_freq_est) / 16) * 20e6 / (2 * M_PI)));
 				break;
 			}
 		}
@@ -117,59 +113,53 @@ int general_work (int noutput, gr_vector_int& ninput_items,
 		break;
 
 	case COPY:
-		while(i < ninput) {
+		while(i < ninput && o < noutput) {
 
 			int rel = d_offset - d_frame_start;
-			if( (rel >= 0) && (rel % 80 > 15)) {
 
-				if(o >= noutput) {
+			if(!rel)  {
-					break;
+				add_item_tag(0, nitems_written(0),
-				}
+					pmt::string_to_symbol("ofdm_start"),
+					pmt::PMT_T,
+					pmt::string_to_symbol(name()));
+			}
 
-				if(rel == 16) {
+			if(rel >= 0 && (rel < 128 || ((rel - 128) % 80) > 15)) {
-					assert(o == 0);
+				out[o] = in_delayed[i] * exp(gr_complex(0, d_offset * d_freq_offset));
-					add_item_tag(0, nitems_written(0) + o,
-						pmt::string_to_symbol("ofdm_start"),
-						pmt::PMT_T,
-						pmt::string_to_symbol(name()));
-				}
-
-				out[o] = in_delayed[i] * exp(gr_complex(0, d_offset * arg(d_freq_est) / 16));
 				o++;
-
 			}
 
 			i++;
 			d_offset++;
+			if(d_offset == 1) {
+				dout << "produced " << d_count << "  " << d_count / 64.0 << std::endl;
+				d_count = 0;
+			}
 		}
 
 		break;
 
-	case RESET:
+	case RESET: {
+		int h = 0;
 		while(o < noutput) {
-			int rel = (d_offset - d_frame_start) % 80;
+			if(((d_count + h) % 64) == 0) {
-
-			if(!rel) {
 				d_offset = 0;
-				d_freq_est = gr_complex(0, 0);
 				d_state = SYNC;
 				break;
-			} else if(rel > 15) {
+			} else {
 				out[o] = 0;
 				o++;
 			}
-			d_offset++;
+			h++;
 		}
 
 		break;
-
+	}
-	default:
-		std::runtime_error("bad state");
-		break;
 	}
 
 	dout << "produced : " << o << " consumed: " << i << std::endl;
 
+	d_count += o;
 	consume(0, i);
 	consume(1, i);
 	return o;
@@ -193,11 +183,10 @@ void search_frame_start() {
 
 	// sort list (highest correlation first)
 	assert(d_cor.size() == SYNC_LENGTH);
-	d_cor.sort();
+	d_cor.sort(compare_abs);
-	d_cor.reverse();
 
 	// copy list in vector for nicer access
-	vector<pair<double, int> > vec(d_cor.begin(), d_cor.end());
+	vector<pair<gr_complex, int> > vec(d_cor.begin(), d_cor.end());
 	d_cor.clear();
 
 	// in case we don't find anything use SYNC_LENGTH
@@ -205,17 +194,30 @@ void search_frame_start() {
 
 	for(int i = 0; i < 3; i++) {
 		for(int k = i + 1; k < 4; k++) {
-			int diff = abs(get<1>(vec[i]) - get<1>(vec[k]));
+			gr_complex first;
+			gr_complex second;
+			if(get<1>(vec[i]) > get<1>(vec[k])) {
+				first = get<0>(vec[k]);
+				second = get<0>(vec[i]);
+			} else {
+				first = get<0>(vec[i]);
+				second = get<0>(vec[k]);
+			}
+			int diff  = abs(get<1>(vec[i]) - get<1>(vec[k]));
 			if(diff == 64) {
-				d_frame_start =  max(get<1>(vec[i]), get<1>(vec[k])) + 64;
+				d_frame_start =  max(get<1>(vec[i]), get<1>(vec[k])) + 64 - 128 - 1;
+				d_freq_offset = arg(first * conj(second)) / 64;
+				//std::cout << "frequency offset " << d_freq_offset << std::endl;
 				// nice match found, return immediately
 				return;
 
 			// TODO: check if these offsets make sense
 			} else if(diff == 63) {
-				d_frame_start = max(get<1>(vec[i]), get<1>(vec[k])) + 63;
+				d_frame_start = max(get<1>(vec[i]), get<1>(vec[k])) + 63 - 128 - 1;
+				d_freq_offset = arg(first * conj(second)) / 64;
 			} else if(diff == 65) {
-				d_frame_start = max(get<1>(vec[i]), get<1>(vec[k])) + 64;
+				d_frame_start = max(get<1>(vec[i]), get<1>(vec[k])) + 64 - 128 - 1;
+				d_freq_offset = arg(first * conj(second)) / 64;
 			}
 		}
 	}
@@ -223,14 +225,15 @@ void search_frame_start() {
 
 private:
 	enum {SYNC, COPY, RESET} d_state;
+	int         d_count;
 	int         d_offset;
 	int         d_frame_start;
-	gr_complex  d_freq_est;
+	float       d_freq_offset;
+
 	gr_complex *d_correlation;
-	gr_complex *d_freq_est_buf;
+	list<pair<gr_complex, int> > d_cor;
-	list<pair<double, int> > d_cor;
 	std::vector<gr::tag_t> d_tags;
-	gr::filter::kernel::fir_filter_ccc *d_fir;
+	gr::filter::kernel::fir_filter_ccc d_fir;
 
 	const bool d_log;
 	const bool d_debug;