diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 53a261eae..27a723118 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -162,6 +162,19 @@ Rtklib_Solver::Rtklib_Solver(const rtk_t &rtk, LOG(WARNING) << "Exception opening RTKLIB dump file " << e.what(); } } + if (d_vtl_dump_file.is_open() == false) + { + try + { + d_vtl_dump_file.exceptions(std::ofstream::failbit | std::ofstream::badbit); + d_vtl_dump_file.open(d_dump_filename + "_vtl", std::ios::out | std::ios::binary); + LOG(INFO) << "PVT VTL dump enabled Log file: " << d_dump_filename + "_vtl"; + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception opening VTL dump file " << e.what(); + } + } } } @@ -195,6 +208,7 @@ Rtklib_Solver::~Rtklib_Solver() try { save_matfile(); + save_vtl_matfile(); } catch (const std::exception &ex) { @@ -204,17 +218,266 @@ Rtklib_Solver::~Rtklib_Solver() } +bool Rtklib_Solver::save_vtl_matfile() const +{ + // READ DUMP FILE + const std::string dump_filename = d_dump_filename+ "_vtl"; + const int32_t number_of_double_vars = 27; + const int32_t number_of_uint32_vars = 2; + const int32_t number_of_uint8_vars = 1; + const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + + sizeof(uint32_t) * number_of_uint32_vars + + sizeof(uint8_t) * number_of_uint8_vars; + std::ifstream dump_file; + 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 VTL dump file:" << e.what() << '\n'; + return false; + } + // count number of epochs and rewind + int64_t num_epoch = 0LL; + if (dump_file.is_open()) + { + std::cout << "Generating .mat file for VTL " << dump_filename << '\n'; + const std::ifstream::pos_type size = dump_file.tellg(); + num_epoch = static_cast(size) / static_cast(epoch_size_bytes); + dump_file.seekg(0, std::ios::beg); + } + else + { + return false; + } + + //todo: cambiarlo + auto TOW_at_current_symbol_ms = std::vector(num_epoch); + auto week = std::vector(num_epoch); + auto RX_time = std::vector(num_epoch); + auto user_clk_offset = std::vector(num_epoch); + auto pos_x = std::vector(num_epoch); + auto pos_y = std::vector(num_epoch); + auto pos_z = std::vector(num_epoch); + auto vel_x = std::vector(num_epoch); + auto vel_y = std::vector(num_epoch); + auto vel_z = std::vector(num_epoch); + auto acc_x = std::vector(num_epoch); + auto acc_y = std::vector(num_epoch); + auto acc_z = std::vector(num_epoch); + auto cov_xx = std::vector(num_epoch); + auto cov_yy = std::vector(num_epoch); + auto cov_zz = std::vector(num_epoch); + auto cov_vx = std::vector(num_epoch); + auto cov_vy = std::vector(num_epoch); + auto cov_vz = std::vector(num_epoch); + auto cov_ax = std::vector(num_epoch); + auto cov_ay = std::vector(num_epoch); + auto cov_az = std::vector(num_epoch); + auto latitude = std::vector(num_epoch); + auto longitude = std::vector(num_epoch); + auto height = std::vector(num_epoch); + auto valid_sats = std::vector(num_epoch); + auto gdop = std::vector(num_epoch); + auto pdop = std::vector(num_epoch); + auto hdop = std::vector(num_epoch); + auto vdop = 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(uint32_t)); + dump_file.read(reinterpret_cast(&week[i]), sizeof(uint32_t)); + dump_file.read(reinterpret_cast(&RX_time[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&user_clk_offset[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pos_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vel_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_x[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_y[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&acc_z[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_xx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_yy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_zz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vx[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vy[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_vz[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_ax[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_ay[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&cov_az[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&valid_sats[i]), sizeof(uint8_t)); + dump_file.read(reinterpret_cast(&gdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&pdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&hdop[i]), sizeof(double)); + dump_file.read(reinterpret_cast(&vdop[i]), sizeof(double)); + } + } + dump_file.close(); + } + catch (const std::ifstream::failure &e) + { + std::cerr << "Problem reading dump file:" << e.what() << '\n'; + return false; + } + + // 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_UINT32, MAT_T_UINT32, 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("week", MAT_C_UINT32, MAT_T_UINT32, 2, dims.data(), week.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("RX_time", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), RX_time.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("user_clk_offset", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), user_clk_offset.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pos_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pos_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vel_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vel_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_x", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_x.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_y", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_y.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("acc_z", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), acc_z.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_xx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_xx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_yy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_yy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_zz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_zz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vx", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vx.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vy", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vy.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_vz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_vz.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_ax", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_ay", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_ay.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("cov_az", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), cov_az.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("longitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), longitude.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("height", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), height.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("valid_sats", MAT_C_UINT8, MAT_T_UINT8, 2, dims.data(), valid_sats.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("gdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), gdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("pdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), pdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("hdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), hdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + matvar = Mat_VarCreate("vdop", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), vdop.data(), 0); + Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE + Mat_VarFree(matvar); + + } + + Mat_Close(matfp); + return true; +} + + bool Rtklib_Solver::save_matfile() const { // READ DUMP FILE const std::string dump_filename = d_dump_filename; const int32_t number_of_double_vars = 23; //+2 MAGL - const int32_t number_of_double_vars_sat = 9; //+ pos(3)/vel(3)/psrange/doppler/CN0 for sat const int32_t number_of_uint32_vars = 2; const int32_t number_of_uint8_vars = 3; const int32_t number_of_float_vars = 2; const int32_t epoch_size_bytes = sizeof(double) * number_of_double_vars + - sizeof(double) * number_of_double_vars_sat * 6 + sizeof(uint32_t) * number_of_uint32_vars + sizeof(uint8_t) * number_of_uint8_vars + sizeof(float) * number_of_float_vars; @@ -259,19 +522,6 @@ bool Rtklib_Solver::save_matfile() const auto cov_xy = std::vector(num_epoch); auto cov_yz = std::vector(num_epoch); auto cov_zx = std::vector(num_epoch); - - auto clk_bias = std::vector(num_epoch); - auto clk_drift = std::vector(num_epoch); - auto sat_posX_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posY_m = std::vector>(6, std::vector(num_epoch)); - auto sat_posZ_m = std::vector>(6, std::vector(num_epoch)); - auto sat_velX = std::vector>(6, std::vector(num_epoch)); - auto sat_velY = std::vector>(6, std::vector(num_epoch)); - auto sat_velZ = std::vector>(6, std::vector(num_epoch)); - auto sat_prg_m = std::vector>(6, std::vector(num_epoch)); - auto sat_dopp_hz = std::vector>(6, std::vector(num_epoch)); - auto sat_CN0_dBhz = std::vector>(6, std::vector(num_epoch)); - auto latitude = std::vector(num_epoch); auto longitude = std::vector(num_epoch); auto height = std::vector(num_epoch); @@ -307,21 +557,6 @@ bool Rtklib_Solver::save_matfile() const dump_file.read(reinterpret_cast(&cov_xy[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_yz[i]), sizeof(double)); dump_file.read(reinterpret_cast(&cov_zx[i]), sizeof(double)); - - dump_file.read(reinterpret_cast(&clk_bias[i]), sizeof(double)); - dump_file.read(reinterpret_cast(&clk_drift[i]), sizeof(double)); - for (uint32_t chan = 0; chan < 6; chan++) - { - dump_file.read(reinterpret_cast(&sat_posX_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posY_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_posZ_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velX[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velY[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_velZ[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_prg_m[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_dopp_hz[chan][i]), sizeof(double)); - dump_file.read(reinterpret_cast(&sat_CN0_dBhz[chan][i]), sizeof(double)); - } dump_file.read(reinterpret_cast(&latitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&longitude[i]), sizeof(double)); dump_file.read(reinterpret_cast(&height[i]), sizeof(double)); @@ -344,34 +579,6 @@ bool Rtklib_Solver::save_matfile() const return false; } - auto sat_posX_m_aux = std::vector(6 * num_epoch); - auto sat_posY_m_aux = std::vector(6 * num_epoch); - auto sat_posZ_m_aux = std::vector(6 * num_epoch); - auto sat_velX_aux = std::vector(6 * num_epoch); - auto sat_velY_aux = std::vector(6 * num_epoch); - auto sat_velZ_aux = std::vector(6 * num_epoch); - auto sat_prg_m_aux = std::vector(6 * num_epoch); - auto sat_dopp_hz_aux = std::vector(6 * num_epoch); - auto sat_CN0_dBhz_aux = std::vector(6 * num_epoch); - - uint32_t k = 0U; - for (int64_t j = 0; j < num_epoch; j++) - { - for (uint32_t i = 0; i < 6; i++) - { - sat_posX_m_aux[k] = sat_posX_m[i][j]; - sat_posY_m_aux[k] = sat_posY_m[i][j]; - sat_posZ_m_aux[k] = sat_posZ_m[i][j]; - sat_velX_aux[k] = sat_velX[i][j]; - sat_velY_aux[k] = sat_velY[i][j]; - sat_velZ_aux[k] = sat_velZ[i][j]; - sat_prg_m_aux[k] = sat_prg_m[i][j]; - sat_dopp_hz_aux[k] = sat_dopp_hz[i][j]; - sat_CN0_dBhz_aux[k] = sat_CN0_dBhz[i][j]; - k++; - } - } - // WRITE MAT FILE mat_t *matfp; matvar_t *matvar; @@ -446,44 +653,6 @@ bool Rtklib_Solver::save_matfile() const Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); - matvar = Mat_VarCreate("clk_bias_s", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_bias.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("clk_drift", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), clk_drift.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - std::array dims_sat{static_cast(6), static_cast(num_epoch)}; - matvar = Mat_VarCreate("sat_posX_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posX_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posY_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posY_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_posZ_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_posZ_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velX", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velX_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velY", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velY_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_velZ", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_velZ_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_prg_m", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_prg_m_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_dopp_hz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_dopp_hz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - matvar = Mat_VarCreate("sat_CN0_dBhz", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims_sat.data(), sat_CN0_dBhz_aux.data(), 0); - Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE - Mat_VarFree(matvar); - - matvar = Mat_VarCreate("latitude", MAT_C_DOUBLE, MAT_T_DOUBLE, 2, dims.data(), latitude.data(), 0); Mat_VarWrite(matfp, matvar, MAT_COMPRESSION_ZLIB); // or MAT_COMPRESSION_NONE Mat_VarFree(matvar); @@ -1713,13 +1882,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ //new_vtl_data.debug_print(); //vtl_data.kf_state.print("kf_state_input"); vtl_engine.vtl_loop(vtl_data); - //vtl_data.kf_state.print("kf_state_output"); - // pvt_sol.rr[0] = vtl_data.kf_state[0]; - // pvt_sol.rr[1] = vtl_data.kf_state[1]; - // pvt_sol.rr[2] = vtl_data.kf_state[2]; - // pvt_sol.rr[3] = vtl_data.kf_state[3]; - // pvt_sol.rr[4] = vtl_data.kf_state[4]; - // pvt_sol.rr[5] = vtl_data.kf_state[5]; + } else { @@ -1827,7 +1990,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ this->set_clock_drift_ppm(clock_drift_ppm); // User clock drift [ppm] d_monitor_pvt.user_clk_drift_ppm = clock_drift_ppm; - + // ######## LOG FILE ######### if (d_flag_dump_enabled == true) { @@ -1877,32 +2040,6 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_double = pvt_sol.qr[5]; d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.rx_dts(0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.rx_dts(1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - for (int n = 0; n < 6; n++) - { - tmp_double = vtl_data.sat_p(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_p(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 0); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 1); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_v(n, 2); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.pr_m(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.doppler_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - tmp_double = vtl_data.sat_CN0_dB_hz(n); - d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); - } - // GEO user position Latitude [deg] tmp_double = this->get_latitude(); d_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); @@ -1934,6 +2071,104 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { LOG(WARNING) << "Exception writing RTKLIB dump file " << e.what(); } + + // VTL (optional) MULTIPLEXED FILE RECORDING - Record results to file + if (enable_vtl == true) + + try + { + + double tmp_double; + uint32_t tmp_uint32; + // TOW + tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // WEEK + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); + d_vtl_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); + // PVT GPS time + tmp_double = gnss_observables_map.cbegin()->second.RX_time; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // User clock offset [s] + //tmp_double = rx_position_and_time[3]; + tmp_double = vtl_data.get_user_clock_offset_s(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // ECEF POS X,Y,X [m] + ECEF VEL X,Y,X [m/s] + ECEF ACC X,Y,X [m/s] (9 x double) + std::vector p_vec_m = vtl_data.get_position_ecef_m(); + tmp_double = p_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + std::vector v_vec_m = vtl_data.get_velocity_ecef_m_s(); + tmp_double = v_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + std::vector a_vec_m = vtl_data.get_accel_ecef_m_s2(); + tmp_double = a_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // position/velocity/acceleration variance/ (units^2) (9 x double) + + std::vector p_var_vec_m = vtl_data.get_position_var_ecef_m(); + tmp_double = p_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = p_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + + std::vector v_var_vec_m = vtl_data.get_velocity_var_ecef_m_s(); + tmp_double = v_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = v_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + vector a_var_vec_m = vtl_data.get_accel_var_ecef_m_s2(); + tmp_double = a_var_vec_m[0]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[1]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + tmp_double = a_var_vec_m[2]; + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // GEO user position Latitude [deg] + tmp_double = this->get_latitude(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Longitude [deg] + tmp_double = this->get_longitude(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + // GEO user position Height [m] + tmp_double = this->get_height(); + d_vtl_dump_file.write(reinterpret_cast(&tmp_double), sizeof(double)); + + // NUMBER OF VALID SATS + d_vtl_dump_file.write(reinterpret_cast(&pvt_sol.ns), sizeof(uint8_t)); + + // GDOP / PDOP / HDOP / VDOP + d_vtl_dump_file.write(reinterpret_cast(&d_dop[0]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[1]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[2]), sizeof(double)); + d_vtl_dump_file.write(reinterpret_cast(&d_dop[3]), sizeof(double)); + } + catch (const std::ofstream::failure &e) + { + LOG(WARNING) << "Exception writing VTL dump file " << e.what(); + } } } } diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h old mode 100644 new mode 100755 index 61ce86660..d6781beb8 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -132,6 +132,7 @@ public: private: bool save_matfile() const; + bool save_vtl_matfile() const; void check_has_orbit_clock_validity(const std::map& obs_map); void get_has_biases(const std::map& obs_map); @@ -152,6 +153,7 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; + std::ofstream d_vtl_dump_file; rtk_t d_rtk{}; nav_t d_nav_data{}; Monitor_Pvt d_monitor_pvt{}; diff --git a/src/algorithms/PVT/libs/vtl_data.cc b/src/algorithms/PVT/libs/vtl_data.cc index f1c5e48b8..ddfb2bc78 100755 --- a/src/algorithms/PVT/libs/vtl_data.cc +++ b/src/algorithms/PVT/libs/vtl_data.cc @@ -16,13 +16,15 @@ #include "vtl_data.h" +#include "vector" +#include "armadillo" Vtl_Data::Vtl_Data() { epoch_tow_s = 0; sample_counter = 0; - kf_state = arma::mat(8,1); - kf_P = arma::mat(8,8); + kf_state = arma::mat(11,1); + kf_P = arma::mat(11,11); } void Vtl_Data::init_storage(int n_sats) @@ -66,3 +68,89 @@ void Vtl_Data::debug_print() //doppler_hz.print("satellite Carrier Dopplers [Hz]"); // carrier_phase_rads.print("satellite accumulated carrier phases [rads]"); } + +std::vector Vtl_Data::get_position_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[0]; + temp[1] = kf_state[1]; + temp[2] = kf_state[2]; + + return temp; +} + +std::vector Vtl_Data::get_velocity_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[3]; + temp[1] = kf_state[4]; + temp[2] = kf_state[5]; + + return temp; +} + +std::vector Vtl_Data::get_accel_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_state[6]; + temp[1] = kf_state[7]; + temp[2] = kf_state[8]; + + return temp; +} +std::vector Vtl_Data::get_position_var_ecef_m() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(0,0); + temp[1] = kf_P(1,1); + temp[2] = kf_P(2,2); + + return temp; +} + +std::vector Vtl_Data::get_velocity_var_ecef_m_s() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(3,3); + temp[1] = kf_P(4,4); + temp[2] = kf_P(5,5); + + return temp; +} + +std::vector Vtl_Data::get_accel_var_ecef_m_s2() +{ + std::vector temp = {42,42,42}; + temp[0] = kf_P(6,6); + temp[1] = kf_P(7,7); + temp[2] = kf_P(8,8); + + return temp; +} + +double Vtl_Data::get_latitude() +{ + + return -1.0; +} + +double Vtl_Data::get_longitude() +{ + + + return -1.0; +} + +double Vtl_Data::get_height() +{ + return -1.0; +} + +double Vtl_Data::get_user_clock_offset_s() +{ + double temp=0; + temp=kf_state[9]; + + return temp; +} + diff --git a/src/algorithms/PVT/libs/vtl_data.h b/src/algorithms/PVT/libs/vtl_data.h index 787c69f13..182098ba2 100755 --- a/src/algorithms/PVT/libs/vtl_data.h +++ b/src/algorithms/PVT/libs/vtl_data.h @@ -22,6 +22,7 @@ #include #include #include +#include /** \addtogroup PVT * \{ */ @@ -61,6 +62,16 @@ public: double epoch_tow_s; // current observation RX time [s] uint64_t sample_counter; // current sample counter associated with RX time [samples from start] void debug_print(); + std::vector get_position_ecef_m(); // get_position_ecef_m + std::vector get_velocity_ecef_m_s(); // get_velocity_ecef_m_s + std::vector get_accel_ecef_m_s2(); // get_accel_ecef_m_s2 + std::vector get_position_var_ecef_m(); // get_position_var_ecef_m + std::vector get_velocity_var_ecef_m_s(); // get_velocity_var_ecef_m_s + std::vector get_accel_var_ecef_m_s2(); // get_accel_var_ecef_m_s2 + double get_latitude(); // get_latitude + double get_longitude(); // get_longitude + double get_height(); // get_height + double get_user_clock_offset_s(); // get_user_clock_offset_s; }; diff --git a/src/algorithms/PVT/libs/vtl_engine.cc b/src/algorithms/PVT/libs/vtl_engine.cc index 56fd0e876..08a212083 100755 --- a/src/algorithms/PVT/libs/vtl_engine.cc +++ b/src/algorithms/PVT/libs/vtl_engine.cc @@ -172,7 +172,8 @@ bool Vtl_Engine::vtl_loop(Vtl_Data& new_data) kf_xerr = kf_K * (kf_yerr); // Error state estimation kf_P_x = (arma::eye(size(kf_P_x)) - kf_K * kf_H) * kf_P_x; // update state estimation error covariance matrix kf_x = kf_x - kf_xerr; // updated state estimation (a priori + error) - + new_data.kf_state = kf_x; + new_data.kf_P = kf_P_x; if(abs(delta_t_vtl)>.5){ kf_xerr.print("kf_xERR: "); cout<<"kf_dt: "<50){ - std::cout << " tracking_cmd TOO FAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; + // std::cout << " tracking_cmd TOO FAR: " + // << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + // << " \n"; }else{ - std::cout << " tracking_cmd NEAR: " - << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" - << " \n"; + // std::cout << " tracking_cmd NEAR: " + // << abs(d_x_old_old(2) - tmp_x(2))<< "Hz" + // << " \n"; //d_x_old_old(2) = tmp_x(2); //replace the Code Phase state }