diff options
Diffstat (limited to 'main.cpp')
| -rw-r--r-- | main.cpp | 126 |
1 files changed, 86 insertions, 40 deletions
@@ -22,6 +22,7 @@ */ #include "OutputUHD.hpp" +#include "pointcloud.hpp" #include "AlignSample.hpp" #include "utils.hpp" #include <zmq.hpp> @@ -58,6 +59,8 @@ size_t read_samples_from_file(FILE* fd, std::vector<complexf>& samples, size_t c AlignSample aligner; +PointCloud cloud(10000); + size_t do_receive(OutputUHD* output_uhd) { std::vector<complexf> samps(samps_per_buffer); @@ -89,52 +92,86 @@ size_t do_receive(OutputUHD* output_uhd) return total_received; } -void find_peak_correlation() +const size_t correlation_length = 16 * 1024; // 8ms at 2048000 + +void push_to_point_cloud(size_t rx_delay) { - FILE* fd = nullptr; //fopen("correlation.debug", "w"); + auto points = aligner.get_samples(correlation_length, rx_delay); - while (running) { - 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; + if (points.first.size() > 0) { + cloud.push_samples(points); + } +} - auto result = aligner.crosscorrelate(correlation_length); - auto& xcs = result.correlation; +size_t find_peak_correlation(size_t correlation_length) +{ + double max_norm = 0.0; + size_t pos_max = 0; + + auto result = aligner.crosscorrelate(correlation_length); + auto& xcs = result.correlation; + + // Find correlation peak + for (size_t offset = 0; offset < xcs.size(); offset++) { + complexf xc = xcs[offset]; + if (std::norm(xc) >= max_norm) { + max_norm = std::norm(xc); + pos_max = offset; + } + } + char msg[512]; + snprintf(msg, 512, "Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n", + std::sqrt(max_norm), + (double)pos_max / (double)samplerate * 1000.0, + pos_max, + 10*std::log(result.rx_power), + 10*std::log(result.tx_power), + result.rx_timestamp, + result.tx_timestamp); + std::cerr << msg; + std::this_thread::sleep_for(std::chrono::microseconds(1)); + + // Eat much more than we correlate, because correlation is slow + aligner.consume(204800); + + return pos_max; +} - // Find correlation peak - for (size_t offset = 0; offset < xcs.size(); offset++) { - complexf xc = xcs[offset]; - if (fd) { - fprintf(fd, "%f ", std::norm(xc)); - } - if (std::norm(xc) >= max_norm) { - max_norm = std::norm(xc); - pos_max = offset; - } +void analyse_correlation() +{ + const size_t num_analyse = 10; + std::vector<double> max_positions(num_analyse); + + while (running) { + for (size_t i = 0; running and i < num_analyse; i++) { + if (aligner.ready(correlation_length)) { + max_positions[i] = find_peak_correlation(correlation_length); } - char msg[512]; - snprintf(msg, 512, "Max correlation is %f at %fms (%zu), with RX %fdB and TX %fdB, RXtime %f, TXtime %f\n", - std::sqrt(max_norm), - (double)pos_max / (double)samplerate * 1000.0, - pos_max, - 10*std::log(result.rx_power), - 10*std::log(result.tx_power), - result.rx_timestamp, - result.tx_timestamp); - if (fd) { - fprintf(fd, "\n%s", msg); + else { + MDEBUG("Waiting for correlation\n"); + aligner.debug(); + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + } + + bool all_identical = true; + + double mean = std::accumulate(max_positions.begin(), max_positions.end(), 0.0) / (double)max_positions.size(); + + for (size_t i = 0; i < num_analyse; i++) { + if (std::fabs(max_positions[i] - mean) > 1) { + all_identical = false; + break; } - std::cerr << msg; - std::this_thread::sleep_for(std::chrono::microseconds(1)); + } - // Eat much more than we correlate, because correlation is slow - aligner.consume(204800); + if (all_identical) { + size_t delay_samples = max_positions[0]; + + push_to_point_cloud(delay_samples); } else { - MDEBUG("Waiting for correlation\n"); - aligner.debug(); - std::this_thread::sleep_for(std::chrono::seconds(1)); + MDEBUG("Not all delays identical\n"); } } } @@ -172,7 +209,7 @@ int main(int argc, char **argv) zmq::socket_t zmq_sock(ctx, ZMQ_SUB); FILE* fd = nullptr; - if (uri == "test") { + if (uri == "test") { //{{{ FILE* fd_rx = fopen("rx.test", "r"); if (!fd_rx) { std::cerr << "fx_rx open error" << std::endl; @@ -228,7 +265,7 @@ int main(int argc, char **argv) aligner.consume(correlation_length / 2); } return 0; - } + } // }}} else if (uri.find("tcp://") != 0) { fd = fopen(uri.c_str(), "rb"); if (!fd) { @@ -256,7 +293,7 @@ int main(int argc, char **argv) std::thread receive_thread(do_receive, &output_uhd); - std::thread correlator_thread(find_peak_correlation); + std::thread correlator_thread(analyse_correlation); do { const double first_sample_time = 4.0; @@ -293,6 +330,15 @@ int main(int argc, char **argv) } total_samps_read += samps_read; + + try { + cloud.handle_event(); + } + catch (sdl_quit &e) { + running = false; + } + + cloud.draw(); } while (samps_read and sent and running); MDEBUG("Leaving main loop with running=%d\n", running ? 1 : 0); |