From 4c94787579c0651e9612c4bd7d5291b0931d3567 Mon Sep 17 00:00:00 2001 From: cmayer Date: Mon, 29 Oct 2018 12:25:56 +0100 Subject: [PATCH] initial doppler estimate before starting the adaptive DFE --- examples/test_cc.grc | 6 +- lib/adaptive_dfe_impl.cc | 264 ++++++++++++++++----------- lib/adaptive_dfe_impl.h | 11 +- python/physical_layer/STANAG_4285.py | 92 ++++++---- 4 files changed, 225 insertions(+), 148 deletions(-) diff --git a/examples/test_cc.grc b/examples/test_cc.grc index 3cd0f58..e9f4478 100644 --- a/examples/test_cc.grc +++ b/examples/test_cc.grc @@ -242,7 +242,7 @@ label0 - PSK + BPSK label1 @@ -282,7 +282,7 @@ option2 - '1' + '2' option3 @@ -802,7 +802,7 @@ repeat - False + True diff --git a/lib/adaptive_dfe_impl.cc b/lib/adaptive_dfe_impl.cc index e90b4a8..18390bc 100644 --- a/lib/adaptive_dfe_impl.cc +++ b/lib/adaptive_dfe_impl.cc @@ -44,7 +44,17 @@ public: PyGILState_Release(_state); } } ; + +boost::python::numpy::ndarray +complex_vector_to_ndarray(std::vector const& v) { + return boost::python::numpy::from_data + (&v.front(), + boost::python::numpy::dtype::get_builtin(), + boost::python::make_tuple(v.size()), + boost::python::make_tuple(sizeof(gr_complex)), + boost::python::object()); } +} // anonymous namespace adaptive_dfe::sptr adaptive_dfe::make(int sps, // samples per symbol @@ -97,6 +107,7 @@ adaptive_dfe_impl::adaptive_dfe_impl(int sps, // samples per symbol , _scramble() , _descrambled_symbols() , _symbol_counter(0) + , _need_samples(false) , _df(0) , _phase(0) , _b{0.338187046465954, -0.288839024460507} @@ -137,13 +148,12 @@ adaptive_dfe_impl::general_work(int noutput_items, for (; i const empty_vec; + // initial doppler estimate + if (!update_doppler_information(_physicalLayer.attr("get_doppler") + (complex_vector_to_ndarray(empty_vec), + complex_vector_to_ndarray(_samples)))) { + _state = WAIT_FOR_PREAMBLE; + continue; + } + } catch (boost::python::error_already_set const&) { + PyErr_Print(); + } + // (1) correct all samples in the circular buffer with the inital doppler estimate + for (int j=_nB+1; j<_nB+_nF+1; ++j) { + assert(_hist_sample_index+j < 2*(_nB+_nF+1)); + _hist_samples[_hist_sample_index+j] *= gr_expj(-_phase); + update_local_oscillator(); + } + // (2) insert all buffered samples and run the adaptive filter for them + // instead of pop_front() we first reverse _samples and then insert back() + pop_back() + // O(N) instead of O(N^2) + std::reverse(_samples.begin(), _samples.end()); + while (!_samples.empty() && nout < noutput_items) { + insert_sample(_samples.back()); + _sample_counter += 1; + _samples.pop_back(); + if ((_sample_counter%_sps) == 0) + out[nout++] = filter(); + } + if (_samples.empty()) { + _state = DO_FILTER; + } else { + _state = INITIAL_DOPPLER_ESTIMATE_CONTINUE; + } + continue; + } + _samples.push_back(in[i]); + } // INITIAL_DOPPLER_ESTIMATE_CONTINUE + + if (_state == INITIAL_DOPPLER_ESTIMATE_CONTINUE) { + std::cout << "INITIAL_DOPPLER_ESTIMATE_CONTINUE\n"; + while (!_samples.empty() && nout < noutput_items) { + insert_sample(_samples.back()); + _sample_counter += 1; + _samples.pop_back(); + if ((_sample_counter%_sps) == 0) + out[nout++] = filter(); + } + if (_samples.empty()) { + _state = DO_FILTER; + } else { + _state = INITIAL_DOPPLER_ESTIMATE_CONTINUE; + } + continue; + } // INITIAL_DOPPLER_ESTIMATE_CONTINUE + if (_state == DO_FILTER) { - gr_complex dot_samples = 0; - // volk_32fc_x2_dot_prod_32fc(&dot_samples, - // _hist_samples+_hist_sample_index, - // _taps_samples, - // _nB+_nF+1); - // if (_sample_counter < 80*5) - // std::cout << "SAMPLE " << _sample_counter << " " << dot_samples << std::endl; - gr_complex filter_output = dot_samples; - _samples.push_back(_hist_samples[_hist_sample_index+_nB+1]); if ((_sample_counter%_sps) == 0) { if (_symbol_counter == _symbols.size()) { _symbol_counter = 0; GILLock gil_lock; try { - boost::python::numpy::ndarray sy = - boost::python::numpy::from_data(&_descrambled_symbols.front(), - boost::python::numpy::dtype::get_builtin(), - boost::python::make_tuple(_descrambled_symbols.size()), - boost::python::make_tuple(sizeof(gr_complex)), - boost::python::object()); - boost::python::numpy::ndarray sa = - boost::python::numpy::from_data(&_samples.front(), - boost::python::numpy::dtype::get_builtin(), - boost::python::make_tuple(_samples.size()), - boost::python::make_tuple(sizeof(gr_complex)), - boost::python::object()); + update_doppler_information(_physicalLayer.attr("get_doppler") + (complex_vector_to_ndarray(_descrambled_symbols), + complex_vector_to_ndarray(_samples))); _samples.clear(); - update_doppler_information(_physicalLayer.attr("get_doppler")(sy, sa)); update_frame_information(_physicalLayer.attr("get_frame")()); } catch (boost::python::error_already_set const&) { PyErr_Print(); } } - gr_complex known_symbol = _symbols[_symbol_counter]; - bool is_known = true; - filter_output = 0; -#if 1 - volk_32fc_x2_dot_prod_32fc(&filter_output, - _hist_samples+_hist_sample_index, - _taps_samples, - _nB+_nF+1); -#else - for (int l=0; l<_nB+_nF+1; ++l) { - assert(_hist_sample_index+l < 2*(_nB+_nF+1)); - filter_output += _hist_samples[_hist_sample_index+l]*_taps_samples[l]; - } -#endif - gr_complex dot_symbols=0; - for (int l=0; l<_nW; ++l) { - assert(_hist_symbol_index+l < 2*_nW); - dot_symbols += _hist_symbols[_hist_symbol_index+l]*_taps_symbols[l]; - } - filter_output += dot_symbols; - if (std::abs(known_symbol) < 1e-5) { // not known - is_known = false; - gr_complex descrambled_filter_output = std::conj(_scramble[_symbol_counter]) * filter_output; - gr::digital::constellation_sptr constell = _constellations[_constellation_index]; - unsigned int jc = constell->decision_maker(&descrambled_filter_output); - gr_complex descrambled_symbol = 0; - constell->map_to_points(jc, &descrambled_symbol); - - // make soft decisions - float const err = std::abs(descrambled_filter_output - descrambled_symbol); - _npwr_counter[_constellation_index] += (_npwr_counter[_constellation_index] < _npwr_max_time_constant); - float const alpha = 1.0f/_npwr_counter[_constellation_index]; - _npwr[_constellation_index] = (1-alpha)*_npwr[_constellation_index] + alpha*err; - std::vector soft_dec = constell->calc_soft_dec(descrambled_filter_output, _npwr[_constellation_index]); - // std::cout << "soft_dec " << _npwr[_constellation_index] << " : "; - // for (int k=0; k tMax) { - // tMax = std::abs(_taps_samples[j]); - // jMax = j; - // } - } - // std::cout << "taps_max: " << jMax << " " << tMax << std::endl; - for (int j=0; j<_nW; ++j) { - assert(_hist_symbol_index+j < 2*_nW); - _taps_symbols[j] -= _mu*err*std::conj(_hist_symbols[_hist_symbol_index+j]) + _alpha*_taps_symbols[j]; - } - // if (_sample_counter < 80*5) - // std::cout << "filter: " << _symbol_counter << " " << _sample_counter << " " << filter_output << " " << known_symbol << " " << std::abs(err) << std::endl; - if (is_known || true) { - _hist_symbols[_hist_symbol_index] = _hist_symbols[_hist_symbol_index + _nW] = known_symbol; - if (++_hist_symbol_index == _nW) - _hist_symbol_index = 0; - } - _descrambled_symbols[_symbol_counter] = filter_output*std::conj(_scramble[_symbol_counter]); - out[nout++] = filter_output*std::conj(_scramble[_symbol_counter]); - ++_symbol_counter; + out[nout++] = filter(); } + insert_sample(in[i]); + if (_need_samples) + _samples.push_back(_hist_samples[_hist_sample_index+_nB+1]); _sample_counter += 1; - } - } + } // DO_FILTER + } // next input sample consume(0, i); @@ -311,7 +299,7 @@ bool adaptive_dfe_impl::start() try { boost::python::object module = boost::python::import(boost::python::str("digitalhf.physical_layer." + _py_module_name)); boost::python::object PhysicalLayer = module.attr("PhysicalLayer"); - _physicalLayer = PhysicalLayer(); + _physicalLayer = PhysicalLayer(_sps); update_constellations(_physicalLayer.attr("get_constellations")()); } catch (boost::python::error_already_set const&) { PyErr_Print(); @@ -331,10 +319,71 @@ bool adaptive_dfe_impl::stop() VOLK_SAFE_DELETE(_hist_symbols); return true; } +gr_complex adaptive_dfe_impl::filter() { + gr_complex filter_output = 0; + volk_32fc_x2_dot_prod_32fc(&filter_output, + _hist_samples+_hist_sample_index, + _taps_samples, + _nB+_nF+1); + gr_complex dot_symbols=0; + for (int l=0; l<_nW; ++l) { + assert(_hist_symbol_index+l < 2*_nW); + dot_symbols += _hist_symbols[_hist_symbol_index+l]*_taps_symbols[l]; + } + filter_output += dot_symbols; + gr_complex known_symbol = _symbols[_symbol_counter]; + bool const is_known = std::abs(known_symbol) > 1e-5; + if (not is_known) { // not known + gr_complex const descrambled_filter_output = std::conj(_scramble[_symbol_counter]) * filter_output; + gr::digital::constellation_sptr constell = _constellations[_constellation_index]; + unsigned int jc = constell->decision_maker(&descrambled_filter_output); + gr_complex descrambled_symbol = 0; + constell->map_to_points(jc, &descrambled_symbol); + + // make soft decisions + float const err = std::abs(descrambled_filter_output - descrambled_symbol); + _npwr_counter[_constellation_index] += (_npwr_counter[_constellation_index] < _npwr_max_time_constant); + float const alpha = 1.0f/_npwr_counter[_constellation_index]; + _npwr[_constellation_index] = (1-alpha)*_npwr[_constellation_index] + alpha*err; + std::vector soft_dec = constell->calc_soft_dec(descrambled_filter_output, _npwr[_constellation_index]); + // std::cout << "soft_dec " << _npwr[_constellation_index] << " : "; + // for (int k=0; k tMax) { + // tMax = std::abs(_taps_samples[j]); + // jMax = j; + // } + } + // std::cout << "taps_max: " << jMax << " " << tMax << std::endl; + for (int j=0; j<_nW; ++j) { + assert(_hist_symbol_index+j < 2*_nW); + _taps_symbols[j] -= _mu*err*std::conj(_hist_symbols[_hist_symbol_index+j]) + _alpha*_taps_symbols[j]; + } + // if (_sample_counter < 80*5) + // std::cout << "filter: " << _symbol_counter << " " << _sample_counter << " " << filter_output << " " << known_symbol << " " << std::abs(err) << std::endl; + if (is_known || true) { + _hist_symbols[_hist_symbol_index] = _hist_symbols[_hist_symbol_index + _nW] = known_symbol; + if (++_hist_symbol_index == _nW) + _hist_symbol_index = 0; + } + _descrambled_symbols[_symbol_counter] = filter_output*std::conj(_scramble[_symbol_counter]); + return filter_output*std::conj(_scramble[_symbol_counter++]); +} void adaptive_dfe_impl::set_mode(std::string mode) { gr::thread::scoped_lock lock(d_setlock); - std::cout << "adaptive_dfe_impl::stop()" << std::endl; + std::cout << "adaptive_dfe_impl::set_mode " << mode << std::endl; GILLock gil_lock; try { _physicalLayer.attr("set_mode")(mode); @@ -342,7 +391,6 @@ void adaptive_dfe_impl::set_mode(std::string mode) { PyErr_Print(); return; } - update_constellations(_physicalLayer.attr("get_constellations")()); } void adaptive_dfe_impl::update_constellations(boost::python::object obj) @@ -368,10 +416,10 @@ void adaptive_dfe_impl::update_constellations(boost::python::object obj) _npwr_counter[i] = 0; } } -void adaptive_dfe_impl::update_frame_information(boost::python::object obj) +bool adaptive_dfe_impl::update_frame_information(boost::python::object obj) { int const n = boost::python::extract(obj.attr("__len__")()); - assert(n==2); + assert(n==3); boost::python::numpy::ndarray array = boost::python::numpy::array(obj[0]); char const* data = array.get_data(); int const m = array.shape(0); @@ -384,9 +432,11 @@ void adaptive_dfe_impl::update_frame_information(boost::python::object obj) std::memcpy(&_scramble[i], data+16*i+8, sizeof(gr_complex)); // std::cout << "get_frame " << i << " " << _symbols[i] << " " << _scramble[i] << std::endl; } - _constellation_index = boost::python::extract(obj[1]); + _constellation_index = boost::python::extract (obj[1]); + _need_samples = boost::python::extract(obj[2]); + return true; } -void adaptive_dfe_impl::update_doppler_information(boost::python::object obj) +bool adaptive_dfe_impl::update_doppler_information(boost::python::object obj) { int const n = boost::python::extract(obj.attr("__len__")()); assert(n==2); @@ -398,10 +448,11 @@ void adaptive_dfe_impl::update_doppler_information(boost::python::object obj) std::fill_n(_hist_samples, 2*(_nB+_nF+1), gr_complex(0)); _hist_sample_index = 0; _sample_counter = 0; - return; + return false; } float const doppler = boost::python::extract(obj[1]); update_pll(doppler); + return true; } void adaptive_dfe_impl::update_pll(float doppler) { @@ -422,8 +473,9 @@ void adaptive_dfe_impl::insert_sample(gr_complex z) { _hist_samples[_hist_sample_index] = _hist_samples[_hist_sample_index+_nB+_nF+1] = z * gr_expj(-_phase); if (++_hist_sample_index == _nB+_nF+1) _hist_sample_index = 0; - - // local oscillator update + update_local_oscillator(); +} +void adaptive_dfe_impl:: update_local_oscillator() { _phase += _df; if (_phase > M_PI) _phase -= 2*M_PI; diff --git a/lib/adaptive_dfe_impl.h b/lib/adaptive_dfe_impl.h index 83ae174..67f4e40 100644 --- a/lib/adaptive_dfe_impl.h +++ b/lib/adaptive_dfe_impl.h @@ -63,6 +63,8 @@ private: std::vector _descrambled_symbols; int _symbol_counter; + bool _need_samples; + // PLL for doppler tracking float _df; // frequency offset in radians per sample float _phase; // accumulated phase for frequency correction @@ -71,12 +73,17 @@ private: enum state { WAIT_FOR_PREAMBLE, + INITIAL_DOPPLER_ESTIMATE, + INITIAL_DOPPLER_ESTIMATE_CONTINUE, DO_FILTER } _state; void update_constellations(boost::python::object obj); - void update_frame_information(boost::python::object obj); - void update_doppler_information(boost::python::object obj); + bool update_frame_information(boost::python::object obj); + bool update_doppler_information(boost::python::object obj); + + void update_local_oscillator(); + gr_complex filter(); void insert_sample(gr_complex z); void update_pll(float doppler); diff --git a/python/physical_layer/STANAG_4285.py b/python/physical_layer/STANAG_4285.py index 1f3982f..7f4ff4d 100644 --- a/python/physical_layer/STANAG_4285.py +++ b/python/physical_layer/STANAG_4285.py @@ -6,56 +6,72 @@ from gnuradio import digital class PhysicalLayer(object): """Physical layer description for STANAG 4285""" - def __init__(self, mode=0): - """For STANAG 4258 the mode has to be set manually: mode=0 -> BPSK, mode=1 -> QPSK, mode=2 -> 8PSK""" - self._constellations = [PhysicalLayer.make_psk(2, [0,1]), - PhysicalLayer.make_psk(4, [0,1,3,2]), - PhysicalLayer.make_psk(8, [1,0,2,3,6,7,5,4])] - self._preamble = [PhysicalLayer.get_preamble(), 0] ## BPSK - self._data = [PhysicalLayer.get_data(), mode] ## according to the mode - self._counter = 0 - self._is_first_frame = True + MODE_BPSK=0 + MODE_QPSK=1 + MODE_8PSK=2 + + def __init__(self, sps): + """intialization""" + self._sps = sps + self._mode = self.MODE_BPSK + self._frame_counter = 0 + self._is_first_frame = True + self._constellations = [self.make_psk(2, [0,1]), + self.make_psk(4, [0,1,3,2]), + self.make_psk(8, [1,0,2,3,6,7,5,4])] + self._preamble = self.get_preamble() + self._data = self.get_data() def set_mode(self, mode): - """For STANAG 4258 the mode has to be set manually: mode=0 -> BPSK, mode=1 -> QPSK, mode=2 -> 8PSK""" + """set phase modultation type""" print('set_mode', mode) - self._data[1] = int(mode) + self._mode = int(mode) def get_constellations(self): return self._constellations def get_frame(self): - """returns the known+unknown symbols and scrambling""" - print('-------------------- get_frame --------------------',self._counter) - return self._preamble if self._counter == 0 else self._data + """returns a tuple describing the frame: + [0] ... known+unknown symbols and scrambling + [1] ... modulation type after descrambling + [2] ... a boolean indicating whethere or not raw IQ samples needed""" + print('-------------------- get_frame --------------------', self._frame_counter) + return [self._preamble,self.MODE_BPSK,True] if self.is_preamble() else [self._data,self._mode,False] - def get_doppler(self, sy, sa): - """used for doppler shift update, for determining which frame to provide next, - and for stopping at end of data/when the signal quality is too low - sy ... equalized symbols; sa ... samples""" - print('-------------------- get_doppler --------------------',self._counter,len(sy),len(sa)) - success,doppler = self.quality_preamble(sy,sa) if self._counter == 0 else self.quality_data(sy) - self._counter = (self._counter+1)&1 if success else 0 - self._is_first_frame = not success + def get_doppler(self, symbols, iq_samples): + """returns a tuple + [0] ... quality flag + [1] ... doppler estimate (rad/symbol) if available""" + print('-------------------- get_doppler --------------------', self._frame_counter,len(symbols),len(iq_samples)) + success,doppler = self.quality_preamble(symbols,iq_samples) if self.is_preamble() else self.quality_data(symbols) + if len(symbols) != 0: + self._frame_counter = (self._frame_counter+1)&1 if success else 0 + self._is_first_frame = not success return success,doppler - def quality_preamble(self, sy, sa): - sps = 5 - zp = [x for x in self._preamble[0]['symb'][9:40] for i in range(sps)] - cc = np.array([np.sum(sa[ i*5:(31+i)*5]*zp) for i in range(49)]) - imax = np.argmax(np.abs(cc[0:18])) - pks = cc[(imax,imax+15,imax+16,imax+31),] - apks = np.abs(pks) - test = np.mean(apks[(0,3),]) > 2*np.mean(apks[(1,2),]) - doppler = np.diff(np.unwrap(np.angle(pks[(0,3),])))[0]/31 if test else 0 - idx = range(80) - if self._is_first_frame: - idx = range(30,80) - z = sy[idx]*np.conj(self._preamble[0]['symb'][idx]) - success = np.sum(np.real(z)<0) < 30 + def is_preamble(self): + return self._frame_counter == 0 + + def quality_preamble(self, symbols, iq_samples): + """quality check and doppler estimation for preamble""" + success = True + doppler = 0 + if len(iq_samples) != 0: + zp = [x for x in self._preamble['symb'][9:40] for i in range(self._sps)] + cc = np.array([np.sum(iq_samples[ i*5:(31+i)*5]*zp) for i in range(49)]) + imax = np.argmax(np.abs(cc[0:18])) + pks = cc[(imax,imax+15,imax+16,imax+31),] + apks = np.abs(pks) + success = np.mean(apks[(0,3),]) > 2*np.mean(apks[(1,2),]) + doppler = np.diff(np.unwrap(np.angle(pks[(0,3),])))[0]/31 if success else 0 + if len(symbols) != 0: + idx = range(30,80) if self._is_first_frame else range(80) + z = symbols[idx]*np.conj(self._preamble['symb'][idx]) + success = np.sum(np.real(z)<0) < 30 return success,doppler def quality_data(self, s): + """quality check for the data frame""" known_symbols = np.mod(range(176),48)>=32 success = np.sum(np.real(s[known_symbols])<0) < 20 return success,0 ## no doppler estimate for data frames @@ -87,7 +103,7 @@ class PhysicalLayer(object): for j in range(3): state = np.concatenate(([np.sum(state&taps)&1], state[0:-1])) a=np.zeros(176, dtype=[('symb',np.complex64), ('scramble', np.complex64)]) - ## PSK-8 modulation + ## 8PSK modulation constellation = PhysicalLayer.make_psk(8,range(8))['points'] a['scramble'] = constellation[p,] known_symbols = np.mod(range(176),48)>=32 @@ -96,11 +112,13 @@ class PhysicalLayer(object): @staticmethod def make_psk(n, gray_code): + """generates n-PSK constellation data""" c = np.zeros(n, dtype=[('points', np.complex64), ('symbols', np.uint8)]) c['points'] = np.exp(2*np.pi*1j*np.array(range(n))/n) c['symbols'] = gray_code return c + ## for now not used (doppler estimation after adaptive filtering does not work) @staticmethod def data_aided_frequency_estimation(x,c): """Data-Aided Frequency Estimation for Burst Digital Transmission,