diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index 75a95731f..9cbecbced 100755 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -205,7 +205,7 @@ 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_double_vars_sat = 0; //+ 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; @@ -258,15 +258,6 @@ bool Rtklib_Solver::save_matfile() const 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); @@ -306,18 +297,7 @@ bool Rtklib_Solver::save_matfile() const 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)); @@ -340,33 +320,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; @@ -450,36 +403,6 @@ bool Rtklib_Solver::save_matfile() const 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); @@ -1417,28 +1340,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ 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));