From dd60970f3261e87826a9d59d8592cc4b568dc32c Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Thu, 19 Nov 2020 09:55:08 +0100 Subject: [PATCH] Improve dump of Telemetry blocks --- docs/changelog.md | 4 + .../gnuradio_blocks/CMakeLists.txt | 1 + .../beidou_b1i_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../beidou_b1i_telemetry_decoder_gs.h | 2 + .../beidou_b3i_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../beidou_b3i_telemetry_decoder_gs.h | 2 + .../galileo_telemetry_decoder_gs.cc | 133 ++++++++++++++++++ .../galileo_telemetry_decoder_gs.h | 2 + .../glonass_l1_ca_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../glonass_l1_ca_telemetry_decoder_gs.h | 2 + .../glonass_l2_ca_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../glonass_l2_ca_telemetry_decoder_gs.h | 2 + .../gps_l1_ca_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../gps_l1_ca_telemetry_decoder_gs.h | 2 + .../gps_l2c_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../gps_l2c_telemetry_decoder_gs.h | 2 + .../gps_l5_telemetry_decoder_gs.cc | 119 ++++++++++++++++ .../gps_l5_telemetry_decoder_gs.h | 2 + .../libs/tlm_dump_reader.cc | 7 +- .../libs/tlm_dump_reader.h | 2 + .../libs/gps_l1_ca_read_telemetry_dump.m | 51 +++---- 21 files changed, 1017 insertions(+), 30 deletions(-) diff --git a/docs/changelog.md b/docs/changelog.md index d0e123d5e..2defafb3d 100644 --- a/docs/changelog.md +++ b/docs/changelog.md @@ -85,6 +85,10 @@ SPDX-FileCopyrightText: 2011-2020 Carles Fernandez-Prades #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for abs #include // for exception #include // for cout #include // for shared_ptr, make_shared +#include #define CRC_ERROR_LIMIT 8 @@ -115,6 +117,118 @@ beidou_b1i_telemetry_decoder_gs::~beidou_b1i_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t beidou_b1i_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -585,12 +699,17 @@ int beidou_b1i_telemetry_decoder_gs::general_work(int noutput_items __attribute_ { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h index 658ec8e5e..73f3c7b05 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.h @@ -74,6 +74,8 @@ private: beidou_b1i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); + int32_t save_matfile() const; + void decode_subframe(float *symbols); void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); void decode_bch15_11_01(const int32_t *bits, std::array &decbits); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc index 02ac150c6..bd5a4c09e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc @@ -28,12 +28,14 @@ #include "gnss_synchro.h" #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for abs #include // for exception #include // for cout #include // for shared_ptr, make_shared +#include #define CRC_ERROR_LIMIT 8 @@ -115,6 +117,118 @@ beidou_b3i_telemetry_decoder_gs::~beidou_b3i_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t beidou_b3i_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -615,12 +729,17 @@ int beidou_b3i_telemetry_decoder_gs::general_work( { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h index d36131a82..d0930c812 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.h @@ -72,6 +72,8 @@ private: beidou_b3i_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); + int32_t save_matfile() const; + void decode_subframe(float *symbols); void decode_word(int32_t word_counter, const float *enc_word_symbols, int32_t *dec_word_symbols); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc index 1625844f9..76ead038a 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.cc @@ -34,6 +34,7 @@ #include "gnss_synchro.h" #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for fmod @@ -224,6 +225,119 @@ galileo_telemetry_decoder_gs::~galileo_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t galileo_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -907,12 +1021,31 @@ int galileo_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + switch (d_frame_type) + { + case 1: + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + break; + case 2: + tmp_int = (current_symbol.Prompt_Q > 0.0 ? 1 : -1); + break; + case 3: + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + break; + default: + tmp_int = 0; + break; + } + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h index b3049c3a3..d4465e7d9 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/galileo_telemetry_decoder_gs.h @@ -82,6 +82,8 @@ private: const int32_t d_nn = 2; // Coding rate 1/n const int32_t d_KK = 7; // Constraint Length + int32_t save_matfile() const; + void viterbi_decoder(float *page_part_symbols, int32_t *page_part_bits); void deinterleaver(int32_t rows, int32_t cols, const float *in, float *out); void decode_INAV_word(float *page_part_symbols, int32_t frame_length); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc index 0c5e5ea44..41c055f5c 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.cc @@ -25,6 +25,7 @@ #include "glonass_gnav_utc_model.h" #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for floor, round @@ -32,6 +33,7 @@ #include // for exception #include // for cout #include // for shared_ptr, make_shared +#include #define CRC_ERROR_LIMIT 6 @@ -108,6 +110,118 @@ glonass_l1_ca_telemetry_decoder_gs::~glonass_l1_ca_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t glonass_l1_ca_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -414,12 +528,17 @@ int glonass_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = d_TOW_at_current_symbol; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = 0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h index c7f523b7a..ebd554bdb 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l1_ca_telemetry_decoder_gs.h @@ -82,6 +82,8 @@ private: const int32_t d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS; + int32_t save_matfile() const; + void decode_string(const double *symbols, int32_t frame_length); // Help with coherent tracking diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc index 09b4bf13c..b878b4ec3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.cc @@ -25,6 +25,7 @@ #include "glonass_gnav_utc_model.h" #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for floor, round @@ -32,6 +33,7 @@ #include // for exception #include // for cout #include // for shared_ptr, make_shared +#include #define CRC_ERROR_LIMIT 6 @@ -108,6 +110,118 @@ glonass_l2_ca_telemetry_decoder_gs::~glonass_l2_ca_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t glonass_l2_ca_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -413,12 +527,17 @@ int glonass_l2_ca_telemetry_decoder_gs::general_work(int noutput_items __attribu { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = d_TOW_at_current_symbol; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = 0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h index b8cfc22fb..66349ccd4 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/glonass_l2_ca_telemetry_decoder_gs.h @@ -80,6 +80,8 @@ private: const int32_t d_symbols_per_preamble = GLONASS_GNAV_PREAMBLE_LENGTH_SYMBOLS; + int32_t save_matfile() const; + void decode_string(const double *symbols, int32_t frame_length); // Storage for incoming data diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc index 3d409149a..31aa744bd 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.cc @@ -24,6 +24,7 @@ #include "gps_utc_model.h" // for Gps_Utc_Model #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for round @@ -31,6 +32,7 @@ #include // for exception #include // for cout #include // for shared_ptr +#include #ifdef COMPILER_HAS_ROTL @@ -130,6 +132,118 @@ gps_l1_ca_telemetry_decoder_gs::~gps_l1_ca_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t gps_l1_ca_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -501,12 +615,17 @@ int gps_l1_ca_telemetry_decoder_gs::general_work(int noutput_items __attribute__ { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_symbol.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_symbol.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_symbol.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h index 566db3e98..2cd52ad2e 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l1_ca_telemetry_decoder_gs.h @@ -72,6 +72,8 @@ private: gps_l1_ca_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); + int32_t save_matfile() const; + bool gps_word_parityCheck(uint32_t gpsword); bool decode_subframe(); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc index 987380a92..6e10ba6e2 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.cc @@ -27,6 +27,7 @@ #include "gps_cnav_utc_model.h" // for Gps_CNAV_Utc_Model #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for bitset @@ -34,6 +35,7 @@ #include // for exception #include // for cout #include // for shared_ptr, make_shared +#include gps_l2c_telemetry_decoder_gs_sptr @@ -92,6 +94,118 @@ gps_l2c_telemetry_decoder_gs::~gps_l2c_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t gps_l2c_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -252,12 +366,17 @@ int gps_l2c_telemetry_decoder_gs::general_work(int noutput_items __attribute__(( { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = d_TOW_at_current_symbol; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_synchro_data.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = d_TOW_at_Preamble; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_synchro_data.Prompt_I > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_synchro_data.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h index d49faf7d3..945936028 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l2c_telemetry_decoder_gs.h @@ -72,6 +72,8 @@ private: gps_l2c_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); + int32_t save_matfile() const; + Gnss_Satellite d_satellite; cnav_msg_decoder_t d_cnav_decoder{}; diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc index 7fdb074eb..90dc5e168 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.cc @@ -26,6 +26,7 @@ #include "gps_cnav_utc_model.h" // for Gps_CNAV_Utc_Model #include #include +#include // for Mat_VarCreate #include // for make_any #include // for mp #include // for std::bitset @@ -33,6 +34,7 @@ #include // for std::exception #include // for std::cout #include // for shared_ptr, make_shared +#include gps_l5_telemetry_decoder_gs_sptr @@ -87,6 +89,118 @@ gps_l5_telemetry_decoder_gs::~gps_l5_telemetry_decoder_gs() LOG(WARNING) << "Exception in destructor closing the dump file " << ex.what(); } } + if (d_dump) + { + try + { + save_matfile(); + } + catch (const std::exception &ex) + { + LOG(WARNING) << "Error saving the .mat file: " << ex.what(); + } + } +} + + +int32_t gps_l5_telemetry_decoder_gs::save_matfile() const +{ + std::ifstream::pos_type size; + const int32_t number_of_double_vars = 2; + const int32_t number_of_int_vars = 2; + const int32_t epoch_size_bytes = sizeof(uint64_t) + sizeof(double) * number_of_double_vars + + sizeof(int32_t) * number_of_int_vars; + std::ifstream dump_file; + std::string dump_filename_ = d_dump_filename; + + std::cout << "Generating .mat file for " << dump_filename_ << '\n'; + dump_file.exceptions(std::ifstream::failbit | std::ifstream::badbit); + try + { + dump_file.open(dump_filename_.c_str(), std::ios::binary | std::ios::ate); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem opening dump file:" << e.what() << '\n'; + return 1; + } + // count number of epochs and rewind + int64_t num_epoch = 0; + if (dump_file.is_open()) + { + size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + if (num_epoch == 0LL) + { + // empty file, exit + return 1; + } + dump_file.seekg(0, std::ios::beg); + } + else + { + return 1; + } + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto tracking_sample_counter = std::vector(num_epoch); + auto TOW_at_Preamble_ms = std::vector(num_epoch); + auto nav_symbol = std::vector(num_epoch); + auto prn = std::vector(num_epoch); + + try + { + if (dump_file.is_open()) + { + for (int64_t i = 0; i < num_epoch; i++) + { + dump_file.read(reinterpret_cast(&TOW_at_current_symbol_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&tracking_sample_counter[i]), sizeof(uint64_t)); + dump_file.read(reinterpret_cast(&TOW_at_Preamble_ms[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&nav_symbol[i]), sizeof(int32_t)); + dump_file.read(reinterpret_cast(&prn[i]), sizeof(int32_t)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return 1; + } + + // WRITE MAT FILE + mat_t *matfp; + matvar_t *matvar; + std::string filename = dump_filename_; + filename.erase(filename.length() - 4, 4); + filename.append(".mat"); + matfp = Mat_CreateVer(filename.c_str(), nullptr, MAT_FT_MAT73); + if (reinterpret_cast(matfp) != nullptr) + { + std::array dims{1, static_cast(num_epoch)}; + matvar = Mat_VarCreate("TOW_at_current_symbol_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_current_symbol_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("tracking_sample_counter", MAT_C_UINT64, MAT_T_UINT64, 2, dims.data(), tracking_sample_counter.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("TOW_at_Preamble_ms", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), TOW_at_Preamble_ms.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("nav_symbol", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), nav_symbol.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("PRN", MAT_C_INT32, MAT_T_INT32, 2, dims.data(), prn.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + } + Mat_Close(matfp); + + return 0; } @@ -263,12 +377,17 @@ int gps_l5_telemetry_decoder_gs::general_work(int noutput_items __attribute__((u { double tmp_double; uint64_t tmp_ulong_int; + int32_t tmp_int; tmp_double = static_cast(d_TOW_at_current_symbol_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); tmp_ulong_int = current_synchro_data.Tracking_sample_counter; d_dump_file.write(reinterpret_cast(&tmp_ulong_int), sizeof(uint64_t)); tmp_double = static_cast(d_TOW_at_Preamble_ms) / 1000.0; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_int = (current_synchro_data.Prompt_Q > 0.0 ? 1 : -1); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); + tmp_int = static_cast(current_synchro_data.PRN); + d_dump_file.write(reinterpret_cast(&tmp_int), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h index 3f283259a..7455514c6 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/gps_l5_telemetry_decoder_gs.h @@ -71,6 +71,8 @@ private: gps_l5_telemetry_decoder_gs(const Gnss_Satellite &satellite, bool dump); + int32_t save_matfile() const; + cnav_msg_decoder_t d_cnav_decoder{}; Gnss_Satellite d_satellite; diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc index caf7c1378..75ba85d3e 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.cc @@ -29,6 +29,8 @@ bool Tlm_Dump_Reader::read_binary_obs() d_dump_file.read(reinterpret_cast(&TOW_at_current_symbol), sizeof(double)); d_dump_file.read(reinterpret_cast(&Tracking_sample_counter), sizeof(uint64_t)); d_dump_file.read(reinterpret_cast(&d_TOW_at_Preamble), sizeof(double)); + d_dump_file.read(reinterpret_cast(&nav_symbol), sizeof(int32_t)); + d_dump_file.read(reinterpret_cast(&prn), sizeof(int32_t)); } catch (const std::ifstream::failure &e) { @@ -53,8 +55,9 @@ bool Tlm_Dump_Reader::restart() int64_t Tlm_Dump_Reader::num_epochs() { std::ifstream::pos_type size; - int number_of_vars_in_epoch = 2; - int epoch_size_bytes = sizeof(double) * number_of_vars_in_epoch + sizeof(uint64_t); + int number_of_double_vars_in_epoch = 2; + int number_of_int_vars_in_epoch = 2; + int epoch_size_bytes = sizeof(double) * number_of_double_vars_in_epoch + sizeof(uint64_t) + sizeof(int32_t) * number_of_int_vars_in_epoch; std::ifstream tmpfile(d_dump_filename.c_str(), std::ios::binary | std::ios::ate); if (tmpfile.is_open()) { diff --git a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h index dcadc5bbd..382b316f8 100644 --- a/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h +++ b/src/tests/unit-tests/signal-processing-blocks/libs/tlm_dump_reader.h @@ -38,6 +38,8 @@ public: double TOW_at_current_symbol; uint64_t Tracking_sample_counter; double d_TOW_at_Preamble; + int32_t nav_symbol; + int32_t prn; private: std::string d_dump_filename; diff --git a/src/utils/matlab/libs/gps_l1_ca_read_telemetry_dump.m b/src/utils/matlab/libs/gps_l1_ca_read_telemetry_dump.m index b20af9b22..6057d3a83 100644 --- a/src/utils/matlab/libs/gps_l1_ca_read_telemetry_dump.m +++ b/src/utils/matlab/libs/gps_l1_ca_read_telemetry_dump.m @@ -21,9 +21,11 @@ function [telemetry] = gps_l1_ca_read_telemetry_dump (filename, count) %% m = nargchk (1,2,nargin); -num_double_vars=4; +num_double_vars=3; double_size_bytes=8; -skip_bytes_each_read=double_size_bytes*num_double_vars; +num_int_vars=2; +int_size_bytes=4; +skip_bytes_each_read=double_size_bytes*num_double_vars+num_int_vars*int_size_bytes; bytes_shift=0; if (m) usage (m); @@ -32,34 +34,27 @@ end if (nargin < 3) count = Inf; end -%loops_counter = fread (f, count, 'uint32',4*12); f = fopen (filename, 'rb'); if (f < 0) else - telemetry.tow_current_symbol_ms = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); - bytes_shift=bytes_shift+double_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved - telemetry.tracking_sample_counter = fread (f, count, 'uint64',skip_bytes_each_read-double_size_bytes); - bytes_shift=bytes_shift+double_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved - telemetry.tow = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); - bytes_shift=bytes_shift+double_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved - telemetry.required_symbols = fread (f, count, 'uint64',skip_bytes_each_read-double_size_bytes); - bytes_shift=bytes_shift+double_size_bytes; - fseek(f,bytes_shift,'bof'); % move to next interleaved - + [x, loops_counter] = fread (f,skip_bytes_each_read); + fseek(f,0,-1); + for i=1:min(count, loops_counter), + telemetry(i).tow_current_symbol_ms = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + telemetry(i).tracking_sample_counter = fread (f, count, 'uint64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + telemetry(i).tow = fread (f, count, 'float64',skip_bytes_each_read-double_size_bytes); + bytes_shift=bytes_shift+double_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + telemetry(i).nav_simbols = fread (f, count, 'int32',skip_bytes_each_read-int_size_bytes); + bytes_shift=bytes_shift+int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + telemetry(i).prn = fread (f, count, 'int32',skip_bytes_each_read-int_size_bytes); + bytes_shift=bytes_shift+int_size_bytes; + fseek(f,bytes_shift,'bof'); % move to next interleaved + end fclose (f); - - %%%%%%%% output vars %%%%%%%% - % { - % double tmp_double; - % tmp_double = current_synchro_data.Preamble_delay_ms; - % d_dump_file.write((char*)&tmp_double, sizeof(double)); - % tmp_double = current_synchro_data.Prn_delay_ms; - % d_dump_file.write((char*)&tmp_double, sizeof(double)); - % tmp_double = current_synchro_data.Preamble_symbol_counter; - % d_dump_file.write((char*)&tmp_double, sizeof(double)); - % } end -