diff options
| -rw-r--r-- | AlignSample.cpp | 101 | ||||
| -rw-r--r-- | AlignSample.hpp | 28 | ||||
| -rw-r--r-- | CMakeLists.txt | 7 | ||||
| -rw-r--r-- | OutputUHD.cpp | 48 | ||||
| -rw-r--r-- | main.cpp | 59 |
5 files changed, 185 insertions, 58 deletions
diff --git a/AlignSample.cpp b/AlignSample.cpp index d2d9339..2fcb830 100644 --- a/AlignSample.cpp +++ b/AlignSample.cpp @@ -22,9 +22,14 @@ */ #include "AlignSample.hpp" +#include <cstring> void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_sample_time) { + if (first_sample_time > 5.1 and first_sample_time < 5.5) { + fwrite(samps, sizeof(complexf), len, fd_tx); + } + std::lock_guard<std::mutex> lock(m_mutex); std::copy(samps, samps + len, std::back_inserter(m_txsamples)); @@ -35,6 +40,10 @@ void AlignSample::push_tx_samples(complexf* samps, size_t len, double first_samp void AlignSample::push_rx_samples(complexf* samps, size_t len, double first_sample_time) { + if (first_sample_time > 5.1 and first_sample_time < 5.5) { + fwrite(samps, sizeof(complexf), len, fd_rx); + } + std::lock_guard<std::mutex> lock(m_mutex); std::copy(samps, samps + len, std::back_inserter(m_rxsamples)); @@ -49,60 +58,120 @@ bool AlignSample::ready(size_t min_samples) return align() and m_rxsamples.size() > min_samples and m_txsamples.size() > min_samples; } -CorrelationResult AlignSample::crosscorrelate(size_t max_offset, size_t len) +CorrelationResult AlignSample::crosscorrelate(size_t len) { - std::vector<complexf> rxsamps; - std::vector<complexf> txsamps; double rx_ts = 0; double tx_ts = 0; + /* The size of the FFT is twice as long as the desired + * correlation length, because the FFT does a circular + * correlation, and we therefore pad half with zeros + */ + const size_t N = 2 * len; + // Do a quick copy, so as to free the mutex { std::lock_guard<std::mutex> lock(m_mutex); if (!align() or m_rxsamples.size() < len or - m_txsamples.size() < len + max_offset) { + m_txsamples.size() < len) { CorrelationResult result(0); return result; } - std::copy(m_rxsamples.begin(), m_rxsamples.begin() + len, std::back_inserter(rxsamps)); - std::copy(m_txsamples.begin(), m_txsamples.begin() + len + max_offset, std::back_inserter(txsamps)); + if (!fftw_init) { + rx_fft_in = fftwf_alloc_complex(N); + memset(rx_fft_in, 0, N * sizeof(fftwf_complex)); + rx_fft_out = fftwf_alloc_complex(N); + memset(rx_fft_out, 0, N * sizeof(fftwf_complex)); + tx_fft_in = fftwf_alloc_complex(N); + memset(tx_fft_in, 0, N * sizeof(fftwf_complex)); + tx_fft_out = fftwf_alloc_complex(N); + memset(tx_fft_out, 0, N * sizeof(fftwf_complex)); + ifft_in = fftwf_alloc_complex(N); + memset(ifft_in, 0, N * sizeof(fftwf_complex)); + ifft_out = fftwf_alloc_complex(N); + memset(ifft_out, 0, N * sizeof(fftwf_complex)); + + rx_fft_plan = fftwf_plan_dft_1d(N, + rx_fft_in, rx_fft_out, + FFTW_FORWARD, FFTW_MEASURE); + + tx_fft_plan = fftwf_plan_dft_1d(N, + tx_fft_in, tx_fft_out, + FFTW_FORWARD, FFTW_MEASURE); + + ifft_plan = fftwf_plan_dft_1d(N, + ifft_in, ifft_out, + FFTW_BACKWARD, FFTW_MEASURE); + + fftw_init = true; + } + + memcpy(rx_fft_in, &m_rxsamples.front(), len * sizeof(fftwf_complex)); + memcpy(tx_fft_in, &m_txsamples.front(), len * sizeof(fftwf_complex)); m_rxsamples.erase(m_rxsamples.begin(), m_rxsamples.begin() + len); - m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len + max_offset); + m_txsamples.erase(m_txsamples.begin(), m_txsamples.begin() + len); rx_ts = m_rx_sample_time(); tx_ts = m_tx_sample_time(); } - CorrelationResult result(max_offset); + CorrelationResult result(len); result.rx_timestamp = rx_ts; result.tx_timestamp = tx_ts; - auto& xcorrs = result.correlation; - // Calculate power - for (auto sample : rxsamps) { - result.rx_power += std::norm(sample); + for (size_t i = 0; i < len; i++) { + // Calculate norm + result.rx_power += rx_fft_in[i][0] * rx_fft_in[i][0] + + rx_fft_in[i][1] * rx_fft_in[i][1]; } result.rx_power = std::sqrt(result.rx_power); - for (auto sample : txsamps) { - result.tx_power += std::norm(sample); + for (size_t i = 0; i < len; i++) { + // Calculate norm + result.tx_power += tx_fft_in[i][0] * tx_fft_in[i][0] + + tx_fft_in[i][1] * tx_fft_in[i][1]; } result.tx_power = std::sqrt(result.tx_power); + // Implement + // corr(a, b) = ifft(fft(a) * conj(fft(b))) + // Attention: circular correlation ! + + fftwf_execute(rx_fft_plan); + + fftwf_execute(tx_fft_plan); + + auto rx_fft = reinterpret_cast<std::complex<float>*>(rx_fft_out); + auto tx_fft = reinterpret_cast<std::complex<float>*>(tx_fft_out); + auto ifft = reinterpret_cast<std::complex<float>*>(ifft_in); + + for (size_t i = 0; i < len; i++) { + ifft[i] = rx_fft[i] * std::conj(tx_fft[i]); + } + + fftwf_execute(ifft_plan); + + auto ifft_out2 = reinterpret_cast<std::complex<float>*>(ifft_out); + for (size_t i = 0; i < len; i++) { + result.correlation[i] = ifft_out2[i]; + } + +#if 0 // Calculate correlation for (size_t offset = 0; offset < max_offset; offset++) { complexf xcorr(0, 0); for (size_t i = 0; i < len; i++) { - xcorr += rxsamps[i] * std::conj(txsamps[i+offset]); + xcorr += rxsamps[i]/rx_power_f * std::conj(txsamps[i+offset])/tx_power_f; } - xcorrs[offset] = xcorr; + result.correlation[offset] = xcorr; } +#endif return result; } diff --git a/AlignSample.hpp b/AlignSample.hpp index 31b5de2..6bfdd6b 100644 --- a/AlignSample.hpp +++ b/AlignSample.hpp @@ -27,6 +27,9 @@ #include <deque> #include <mutex> #include <complex> +#include "fftw3.h" +#define FFT_TYPE fftwf_complex + typedef std::complex<float> complexf; @@ -39,8 +42,8 @@ struct CorrelationResult { tx_timestamp(0) {} std::vector<complexf> correlation; - double rx_power; - double tx_power; + float rx_power; + float tx_power; double rx_timestamp; double tx_timestamp; @@ -53,6 +56,11 @@ class AlignSample { m_first_tx_sample_time = 0; m_num_rx_samples_dropped = 0; m_num_tx_samples_dropped = 0; + fftw_init = false; + + fd_rx = fopen("rx.debug", "wb"); + fd_tx = fopen("tx.debug", "wb"); + } void push_tx_samples(complexf* samps, size_t len, double first_sample_time); @@ -68,7 +76,7 @@ class AlignSample { MDEBUG(" TX: %f %zu\n", m_tx_sample_time(), m_txsamples.size()); } - CorrelationResult crosscorrelate(size_t max_offset, size_t len); + CorrelationResult crosscorrelate(size_t len); void consume(size_t samples); @@ -93,6 +101,20 @@ class AlignSample { double m_first_tx_sample_time; size_t m_num_tx_samples_dropped; std::deque<complexf> m_txsamples; + + FILE* fd_rx; + FILE* fd_tx; + + bool fftw_init; + fftwf_complex *rx_fft_in; + fftwf_complex *rx_fft_out; + fftwf_complex *tx_fft_in; + fftwf_complex *tx_fft_out; + fftwf_complex *ifft_in; + fftwf_complex *ifft_out; + fftwf_plan rx_fft_plan; + fftwf_plan tx_fft_plan; + fftwf_plan ifft_plan; }; diff --git a/CMakeLists.txt b/CMakeLists.txt index 891b853..59067e1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ add_definitions(-Wall) find_package(PkgConfig) pkg_check_modules(UHD uhd) pkg_check_modules(ZMQ libzmq) +pkg_check_modules(FFTW3F fftw3f) # Threads find_package(Threads REQUIRED) @@ -65,7 +66,11 @@ list(APPEND odrdpd_sources AlignSample.cpp ) -list(APPEND common_link_list ${UHD_LIBRARIES} ${ZMQ_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) +list(APPEND common_link_list + ${UHD_LIBRARIES} + ${ZMQ_LIBRARIES} + ${FFTW3F_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT}) set_source_files_properties( ${odrdpd_sources} diff --git a/OutputUHD.cpp b/OutputUHD.cpp index fff3e15..6800daf 100644 --- a/OutputUHD.cpp +++ b/OutputUHD.cpp @@ -52,8 +52,12 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) : m_usrp->set_rx_rate(m_samplerate); m_usrp->set_tx_freq(234.208e6); - double set_frequency = m_usrp->get_tx_freq(); - MDEBUG("OutputUHD:Actual frequency: %f\n", set_frequency); + double set_tx_frequency = m_usrp->get_tx_freq(); + MDEBUG("OutputUHD:Actual TX frequency: %f\n", set_tx_frequency); + + m_usrp->set_rx_freq(234.208e6); + double set_rx_frequency = m_usrp->get_rx_freq(); + MDEBUG("OutputUHD:Actual RX frequency: %f\n", set_rx_frequency); MDEBUG("OutputUHD:Setting TX Gain: %f ...\n", m_txgain); m_usrp->set_tx_gain(m_txgain); @@ -76,10 +80,8 @@ OutputUHD::OutputUHD(double txgain, double rxgain, double samplerate) : myTxStream = m_usrp->get_tx_stream(stream_args); myRxStream = m_usrp->get_rx_stream(stream_args); - uhd::stream_cmd_t stream_cmd(true ? - uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS: - uhd::stream_cmd_t::STREAM_MODE_NUM_SAMPS_AND_DONE - ); + uhd::stream_cmd_t stream_cmd( + uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS); stream_cmd.num_samps = 0; stream_cmd.stream_now = true; stream_cmd.time_spec = uhd::time_spec_t(); @@ -115,33 +117,23 @@ size_t OutputUHD::Transmit(const complexf *samples, size_t sizeIn, double *first size_t OutputUHD::Receive(complexf *samples, size_t sizeIn, double *first_sample_time) { const double rx_timeout = 20.0; + const size_t usrp_max_num_samps = myRxStream->get_max_num_samps(); + const size_t samps_to_rx = std::min(sizeIn, usrp_max_num_samps); uhd::rx_metadata_t md; - size_t usrp_max_num_samps = myRxStream->get_max_num_samps(); - - *first_sample_time = md.time_spec.get_real_secs(); + size_t num_rx_samps = myRxStream->recv(samples, samps_to_rx, md, rx_timeout); - size_t num_acc_samps = 0; //number of accumulated samples - while (num_acc_samps < sizeIn) { - size_t samps_to_send = std::min(sizeIn - num_acc_samps, usrp_max_num_samps); + if (md.error_code != uhd::rx_metadata_t::ERROR_CODE_NONE){ + MDEBUG("RX Error %s\n", md.strerror().c_str()); + } - //send a single packet - size_t num_rx_samps = myRxStream->recv( - &samples[num_acc_samps], - samps_to_send, md, rx_timeout); - - if (num_acc_samps == 0) { - if (md.has_time_spec) { - *first_sample_time = md.time_spec.get_real_secs(); - } - else { - MDEBUG("samp %zu no time spec!\n", num_acc_samps); - } - } - - num_acc_samps += num_rx_samps; + if (md.has_time_spec) { + *first_sample_time = md.time_spec.get_real_secs(); + } + else { + *first_sample_time = 0; } - return num_acc_samps; + return num_rx_samps; } @@ -40,7 +40,7 @@ void sig_int_handler(int) { running = false; } -size_t read_samples(FILE* fd, std::vector<complexf>& samples, size_t count) +size_t read_samples_from_file(FILE* fd, std::vector<complexf>& samples, size_t count) { if (samples.size() < count) { MDEBUG("HAD TO RESIZE BUFFER!\n"); @@ -87,14 +87,12 @@ void find_peak_correlation() FILE* fd = fopen("correlation.debug", "w"); while (running) { - const size_t max_offset = 10000; // 4.8ms at 2048000 - if (aligner.ready(max_offset)) { - const size_t correlation_length = 1000; - std::vector<complexf> correlations(max_offset); + const size_t correlation_length = 16 * 1024; // 8ms at 2048000 + if (aligner.ready(correlation_length)) { double max_norm = 0.0; size_t pos_max = 0; - auto result = aligner.crosscorrelate(max_offset, correlation_length); + auto result = aligner.crosscorrelate(correlation_length); auto& xcs = result.correlation; fprintf(fd, "Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n", @@ -162,13 +160,51 @@ int main(int argc, char **argv) std::string uri = argv[1]; - OutputUHD output_uhd(txgain, rxgain, samplerate); - zmq::context_t ctx; zmq::socket_t zmq_sock(ctx, ZMQ_SUB); FILE* fd = nullptr; - if (uri.find("tcp://") != 0) { + if (uri == "test") { + FILE* fd_rx = fopen("rx.debug", "r"); + FILE* fd_tx = fopen("tx.debug", "r"); + + const size_t len = 64; + + std::vector<complexf> rx_samples(len*1024); + std::vector<complexf> tx_samples(len*1024); + + size_t num_rx_samples = fread(&rx_samples.front(), sizeof(complexf), len, fd_rx); + size_t num_tx_samples = fread(&tx_samples.front(), sizeof(complexf), len, fd_tx); + + aligner.push_rx_samples(&rx_samples.front(), num_rx_samples, 1); + aligner.push_tx_samples(&tx_samples.front(), num_tx_samples, 1); + + aligner.debug(); + const size_t correlation_length = 16 * 1024; + double max_norm = 0.0; + size_t pos_max = 0; + + auto result = aligner.crosscorrelate(correlation_length); + auto& xcs = result.correlation; + for (size_t offset = 0; offset < xcs.size(); offset++) { + complexf xc = xcs[offset]; + fprintf(fd, "%f ", std::norm(xc)); + if (std::norm(xc) >= max_norm) { + max_norm = std::norm(xc); + pos_max = offset; + } + } + MDEBUG("Max correlation is %f at %fms, with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n", + std::sqrt(max_norm), + (double)pos_max / (double)samplerate * 1000.0, + 10*std::log(result.rx_power), + 10*std::log(result.tx_power), + result.rx_timestamp, + result.tx_timestamp); + + return 0; + } + else if (uri.find("tcp://") != 0) { fd = fopen(uri.c_str(), "rb"); if (!fd) { MDEBUG("Could not open file\n"); @@ -180,6 +216,9 @@ int main(int argc, char **argv) zmq_sock.setsockopt(ZMQ_SUBSCRIBE, NULL, 0); } + OutputUHD output_uhd(txgain, rxgain, samplerate); + + std::vector<complexf> input_samples(samps_per_buffer); size_t samps_read = 0; size_t total_samps_read = samps_read; @@ -199,7 +238,7 @@ int main(int argc, char **argv) double first_sample_time = 0; if (fd) { - samps_read = read_samples(fd, input_samples, samps_per_buffer); + samps_read = read_samples_from_file(fd, input_samples, samps_per_buffer); sent = output_uhd.Transmit(&input_samples.front(), samps_read, &first_sample_time); aligner.push_tx_samples(&input_samples.front(), samps_read, first_sample_time); } |