From bcad6db988f499b545b9da1abb82e5eef260ae28 Mon Sep 17 00:00:00 2001 From: Vlad P Date: Thu, 14 Jul 2022 12:45:54 +0300 Subject: [PATCH 1/5] Implement processing of BeiDou PRN34..PRN63 Signed-off-by: Vlad P --- src/algorithms/libs/beidou_b1i_signal_replica.cc | 14 ++++++-------- src/algorithms/libs/rtklib/rtklib.h | 2 +- .../system_parameters/beidou_dnav_ephemeris.cc | 2 +- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/algorithms/libs/beidou_b1i_signal_replica.cc b/src/algorithms/libs/beidou_b1i_signal_replica.cc index 513a78cac..88e22c78b 100644 --- a/src/algorithms/libs/beidou_b1i_signal_replica.cc +++ b/src/algorithms/libs/beidou_b1i_signal_replica.cc @@ -26,11 +26,9 @@ const auto AUX_CEIL = [](float x) { return static_cast(static_cast dest, int32_t prn, uint32_t chip_shift) { constexpr uint32_t code_length = 2046; - const std::array delays = {712 /*PRN1*/, 1581, 1414, 1550, 581, 771, 1311, 1043, 1549, 359, 710, 1579, 1548, 1103, 579, 769, 358, 709, 1411, 1547, - 1102, 578, 357, 1577, 1410, 1546, 1101, 707, 1576, 1409, 1545, 354 /*PRN32*/, - 705}; - const std::array phase1 = {1, 1, 1, 1, 1, 1, 1, 1, 2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 8, 8, 8, 9, 9, 10}; - const std::array phase2 = {3, 4, 5, 6, 8, 9, 10, 11, 7, 4, 5, 6, 8, 9, 10, 11, 5, 6, 8, 9, 10, 11, 6, 8, 9, 10, 11, 8, 9, 10, 11, 9, 10, 11, 10, 11, 11}; + const std::array phase1 = {1, 1, 1, 1, 1, 1, 1, 1, 2, 3, 3, 3, 3, 3, 3, 3, 4, 4, 4, 4, 4, 4, 5, 5, 5, 5, 5, 6, 6, 6, 6, 8, 8, 8, 9, 9, 10, 2, 3, 3, 3, 3, 3, 4, 4, 5, 5, 5, 5, 6, 8, 9, 9, 3, 5, 7, 4, 4, 5, 5, 5, 5, 6}; + const std::array phase2 = {3, 4, 5, 6, 8, 9, 10, 11, 7, 4, 5, 6, 8, 9, 10, 11, 5, 6, 8, 9, 10, 11, 6, 8, 9, 10, 11, 8, 9, 10, 11, 9, 10, 11, 10, 11, 11, 7, 4, 6, 8, 10, 11, 5, 9, 6, 8, 10, 11, 9, 9, 10, 11, 7, 7, 9, 5, 9, 6, 8, 10, 11, 9}; + const std::array phase3 = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3}; std::bitset G1{}; std::bitset G2{}; @@ -50,7 +48,7 @@ void beidou_b1i_code_gen_int(own::span dest, int32_t prn, uint32_t chip prn_idx = prn - 1; // A simple error check - if ((prn_idx < 0) || (prn_idx > 32)) + if ((prn_idx < 0) || (prn_idx > 62)) { return; } @@ -59,7 +57,7 @@ void beidou_b1i_code_gen_int(own::span dest, int32_t prn, uint32_t chip for (lcv = 0; lcv < code_length; lcv++) { G1[lcv] = G1_register[0]; - G2[lcv] = G2_register[-(phase1[prn_idx] - 11)] xor G2_register[-(phase2[prn_idx] - 11)]; + G2[lcv] = G2_register[-(phase1[prn_idx] - 11)] xor G2_register[-(phase2[prn_idx] - 11)] xor (phase3[prn_idx] ? G2_register[-(phase3[prn_idx] - 11)] : 0); feedback1 = G1_register[0] xor G1_register[1] xor G1_register[2] xor G1_register[3] xor G1_register[4] xor G1_register[10]; feedback2 = G2_register[0] xor G2_register[2] xor G2_register[3] xor G2_register[6] xor G2_register[7] xor G2_register[8] xor G2_register[9] xor G2_register[10]; @@ -75,7 +73,7 @@ void beidou_b1i_code_gen_int(own::span dest, int32_t prn, uint32_t chip } // Set the delay - delay = code_length - delays[prn_idx] * 0; // ********************************* + delay = code_length; //********************************** delay += chip_shift; delay %= code_length; diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index 379e75b75..216183d48 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -202,7 +202,7 @@ const int NSYSQZS = 0; #define ENABDS #ifdef ENABDS const int MINPRNBDS = 1; //!< min satellite sat number of BeiDou -const int MAXPRNBDS = 37; //!< max satellite sat number of BeiDou +const int MAXPRNBDS = 63; //!< max satellite sat number of BeiDou const int NSATBDS = (MAXPRNBDS - MINPRNBDS + 1); //!< number of BeiDou satellites const int NSYSBDS = 1; #else diff --git a/src/core/system_parameters/beidou_dnav_ephemeris.cc b/src/core/system_parameters/beidou_dnav_ephemeris.cc index b3a5e41ee..0b12e7754 100644 --- a/src/core/system_parameters/beidou_dnav_ephemeris.cc +++ b/src/core/system_parameters/beidou_dnav_ephemeris.cc @@ -22,7 +22,7 @@ Beidou_Dnav_Ephemeris::Beidou_Dnav_Ephemeris() { auto gnss_sat = Gnss_Satellite(); const std::string _system("Beidou"); - for (unsigned int i = 1; i < 36; i++) + for (unsigned int i = 1; i < 64; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); } From 1153544fca55e624565bf10a584162e39b774405 Mon Sep 17 00:00:00 2001 From: Vladisslav P Date: Wed, 6 Jul 2022 03:15:23 +0300 Subject: [PATCH 2/5] Implement support of new GEO BeiDou satellites Update tracking blocks to support PRN59...PRN63 Update telemetry decoder block to support PRN59...PRN63 Update RTKLIB to support PRN59...PRN63 Signed-off-by: Vladisslav P --- src/algorithms/libs/rtklib/rtklib_ephemeris.cc | 2 +- .../gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc | 4 ++-- .../gnuradio_blocks/beidou_b3i_telemetry_decoder_gs.cc | 2 +- .../tracking/gnuradio_blocks/dll_pll_veml_tracking.cc | 4 ++-- .../tracking/gnuradio_blocks/kf_vtl_tracking.cc | 4 ++-- .../system_parameters/beidou_dnav_navigation_message.cc | 8 ++++---- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 1aed8663e..7d821f21b 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -284,7 +284,7 @@ void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts, cosi = cos(i); /* beidou geo satellite (ref [9]) */ - if (sys == SYS_BDS && prn <= 5) + if (sys == SYS_BDS && (prn <= 5 || prn > 58)) { O = eph->OMG0 + eph->OMGd * tk - omge * eph->toes; sinO = sin(O); diff --git a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc index 2de471af1..729dd2af3 100644 --- a/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc +++ b/src/algorithms/telemetry_decoder/gnuradio_blocks/beidou_b1i_telemetry_decoder_gs.cc @@ -264,7 +264,7 @@ void beidou_b1i_telemetry_decoder_gs::decode_subframe(float *frame_symbols) d_nav_msg_packet.nav_message = data_bits; } - if (d_satellite.get_PRN() > 0 && d_satellite.get_PRN() < 6) + if ((d_satellite.get_PRN() > 0 && d_satellite.get_PRN() < 6) || d_satellite.get_PRN() > 58) { d_nav.d2_subframe_decoder(data_bits); } @@ -339,7 +339,7 @@ void beidou_b1i_telemetry_decoder_gs::set_satellite(const Gnss_Satellite &satell d_nav.set_signal_type(1); // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) // Update tel dec parameters for D2 NAV Messages - if (sat_prn > 0 && sat_prn < 6) + if ((sat_prn > 0 && sat_prn < 6) || sat_prn > 58) { d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; 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 b5c93428f..0ea53fad5 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 @@ -356,7 +356,7 @@ void beidou_b3i_telemetry_decoder_gs::set_satellite( d_nav.set_signal_type(5); // BDS: data source (0:unknown,1:B1I,2:B1Q,3:B2I,4:B2Q,5:B3I,6:B3Q) // Update tel dec parameters for D2 NAV Messages - if (sat_prn > 0 && sat_prn < 6) + if ((sat_prn > 0 && sat_prn < 6) || sat_prn > 58) { d_symbols_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; d_samples_per_preamble = BEIDOU_DNAV_PREAMBLE_LENGTH_SYMBOLS; diff --git a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc index aa0f23e63..f73ffe782 100644 --- a/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/dll_pll_veml_tracking.cc @@ -757,7 +757,7 @@ void dll_pll_veml_tracking::start_tracking() { beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // GEO Satellites use different secondary code - if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or (d_acquisition_gnss_synchro->PRN > 58)) { d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization d_correlation_length_ms = 1; @@ -790,7 +790,7 @@ void dll_pll_veml_tracking::start_tracking() { beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites - if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or (d_acquisition_gnss_synchro->PRN > 58)) { d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization d_correlation_length_ms = 1; diff --git a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc index 69548b4ac..a01caaf99 100644 --- a/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc +++ b/src/algorithms/tracking/gnuradio_blocks/kf_vtl_tracking.cc @@ -732,7 +732,7 @@ void kf_vtl_tracking::start_tracking() { beidou_b1i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // GEO Satellites use different secondary code - if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or d_acquisition_gnss_synchro->PRN > 58) { d_symbols_per_bit = BEIDOU_B1I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization d_correlation_length_ms = 1; @@ -765,7 +765,7 @@ void kf_vtl_tracking::start_tracking() { beidou_b3i_code_gen_float(d_tracking_code, d_acquisition_gnss_synchro->PRN, 0); // Update secondary code settings for geo satellites - if (d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) + if ((d_acquisition_gnss_synchro->PRN > 0 and d_acquisition_gnss_synchro->PRN < 6) or d_acquisition_gnss_synchro->PRN > 58) { d_symbols_per_bit = BEIDOU_B3I_GEO_TELEMETRY_SYMBOLS_PER_BIT; // todo: enable after fixing beidou symbol synchronization d_correlation_length_ms = 1; diff --git a/src/core/system_parameters/beidou_dnav_navigation_message.cc b/src/core/system_parameters/beidou_dnav_navigation_message.cc index a9eecc2e5..78e9570dc 100644 --- a/src/core/system_parameters/beidou_dnav_navigation_message.cc +++ b/src/core/system_parameters/beidou_dnav_navigation_message.cc @@ -27,11 +27,11 @@ Beidou_Dnav_Navigation_Message::Beidou_Dnav_Navigation_Message() { auto gnss_sat = Gnss_Satellite(); const std::string _system("Beidou"); - for (uint32_t i = 1; i < 36; i++) + for (uint32_t i = 1; i < 64; i++) { satelliteBlock[i] = gnss_sat.what_block(_system, i); } - for (uint32_t i = 1; i < 36; i++) + for (uint32_t i = 1; i < 64; i++) { almanacHealth[i] = 0; } @@ -647,7 +647,7 @@ Beidou_Dnav_Ephemeris Beidou_Dnav_Navigation_Message::get_ephemeris() const { Beidou_Dnav_Ephemeris eph; - if (i_satellite_PRN > 0 and i_satellite_PRN < 6) + if ((i_satellite_PRN > 0 and i_satellite_PRN < 6) or i_satellite_PRN > 58) { std::bitset subframe_bits; @@ -785,7 +785,7 @@ Beidou_Dnav_Utc_Model Beidou_Dnav_Navigation_Message::get_utc_model() bool Beidou_Dnav_Navigation_Message::have_new_ephemeris() // Check if we have a new ephemeris stored in the galileo navigation class { - if (i_satellite_PRN > 0 and i_satellite_PRN < 6) + if ((i_satellite_PRN > 0 and i_satellite_PRN < 6) or i_satellite_PRN > 58) { if ((flag_sf1_p1 == true) and (flag_sf1_p2 == true) and (flag_sf1_p3 == true) and (flag_sf1_p4 == true) and (flag_sf1_p5 == true) and (flag_sf1_p6 == true) and From 59c9c6f8ab888ee066d838e04eb6391349e93778 Mon Sep 17 00:00:00 2001 From: Vladisslav P Date: Thu, 14 Jul 2022 05:44:08 +0300 Subject: [PATCH 3/5] RTKLIB: Switch to STL containers ...to prevent thread stack abuse. And free up some stack space in Rtklib_Solver::get_PVT. Signed-off-by: Vladisslav P --- src/algorithms/PVT/libs/rtklib_solver.cc | 124 +++++++++--------- src/algorithms/PVT/libs/rtklib_solver.h | 1 + .../libs/rtklib/rtklib_ephemeris.cc | 3 +- src/algorithms/libs/rtklib/rtklib_ionex.cc | 7 +- src/algorithms/libs/rtklib/rtklib_pntpos.cc | 13 +- src/algorithms/libs/rtklib/rtklib_ppp.cc | 21 +-- src/algorithms/libs/rtklib/rtklib_preceph.cc | 7 +- src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 19 ++- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 43 +++--- src/algorithms/libs/rtklib/rtklib_solution.cc | 13 +- 10 files changed, 134 insertions(+), 117 deletions(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.cc b/src/algorithms/PVT/libs/rtklib_solver.cc index aa07a83cc..f84b3506b 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.cc +++ b/src/algorithms/PVT/libs/rtklib_solver.cc @@ -463,7 +463,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ std::map::const_iterator glonass_gnav_ephemeris_iter; std::map::const_iterator beidou_ephemeris_iter; - const Glonass_Gnav_Utc_Model gnav_utc = this->glonass_gnav_utc_model; + const Glonass_Gnav_Utc_Model &gnav_utc = this->glonass_gnav_utc_model; this->set_averaging_flag(flag_averaging); @@ -900,89 +900,89 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ if ((valid_obs + glo_valid_obs) > 3) { int result = 0; - nav_t nav_data{}; - nav_data.eph = eph_data.data(); - nav_data.geph = geph_data.data(); - nav_data.n = valid_obs; - nav_data.ng = glo_valid_obs; + d_nav_data = {}; + d_nav_data.eph = eph_data.data(); + d_nav_data.geph = geph_data.data(); + d_nav_data.n = valid_obs; + d_nav_data.ng = glo_valid_obs; if (gps_iono.valid) { - nav_data.ion_gps[0] = gps_iono.alpha0; - nav_data.ion_gps[1] = gps_iono.alpha1; - nav_data.ion_gps[2] = gps_iono.alpha2; - nav_data.ion_gps[3] = gps_iono.alpha3; - nav_data.ion_gps[4] = gps_iono.beta0; - nav_data.ion_gps[5] = gps_iono.beta1; - nav_data.ion_gps[6] = gps_iono.beta2; - nav_data.ion_gps[7] = gps_iono.beta3; + d_nav_data.ion_gps[0] = gps_iono.alpha0; + d_nav_data.ion_gps[1] = gps_iono.alpha1; + d_nav_data.ion_gps[2] = gps_iono.alpha2; + d_nav_data.ion_gps[3] = gps_iono.alpha3; + d_nav_data.ion_gps[4] = gps_iono.beta0; + d_nav_data.ion_gps[5] = gps_iono.beta1; + d_nav_data.ion_gps[6] = gps_iono.beta2; + d_nav_data.ion_gps[7] = gps_iono.beta3; } if (!(gps_iono.valid) and gps_cnav_iono.valid) { - nav_data.ion_gps[0] = gps_cnav_iono.alpha0; - nav_data.ion_gps[1] = gps_cnav_iono.alpha1; - nav_data.ion_gps[2] = gps_cnav_iono.alpha2; - nav_data.ion_gps[3] = gps_cnav_iono.alpha3; - nav_data.ion_gps[4] = gps_cnav_iono.beta0; - nav_data.ion_gps[5] = gps_cnav_iono.beta1; - nav_data.ion_gps[6] = gps_cnav_iono.beta2; - nav_data.ion_gps[7] = gps_cnav_iono.beta3; + d_nav_data.ion_gps[0] = gps_cnav_iono.alpha0; + d_nav_data.ion_gps[1] = gps_cnav_iono.alpha1; + d_nav_data.ion_gps[2] = gps_cnav_iono.alpha2; + d_nav_data.ion_gps[3] = gps_cnav_iono.alpha3; + d_nav_data.ion_gps[4] = gps_cnav_iono.beta0; + d_nav_data.ion_gps[5] = gps_cnav_iono.beta1; + d_nav_data.ion_gps[6] = gps_cnav_iono.beta2; + d_nav_data.ion_gps[7] = gps_cnav_iono.beta3; } if (galileo_iono.ai0 != 0.0) { - nav_data.ion_gal[0] = galileo_iono.ai0; - nav_data.ion_gal[1] = galileo_iono.ai1; - nav_data.ion_gal[2] = galileo_iono.ai2; - nav_data.ion_gal[3] = 0.0; + d_nav_data.ion_gal[0] = galileo_iono.ai0; + d_nav_data.ion_gal[1] = galileo_iono.ai1; + d_nav_data.ion_gal[2] = galileo_iono.ai2; + d_nav_data.ion_gal[3] = 0.0; } if (beidou_dnav_iono.valid) { - nav_data.ion_cmp[0] = beidou_dnav_iono.alpha0; - nav_data.ion_cmp[1] = beidou_dnav_iono.alpha1; - nav_data.ion_cmp[2] = beidou_dnav_iono.alpha2; - nav_data.ion_cmp[3] = beidou_dnav_iono.alpha3; - nav_data.ion_cmp[4] = beidou_dnav_iono.beta0; - nav_data.ion_cmp[5] = beidou_dnav_iono.beta0; - nav_data.ion_cmp[6] = beidou_dnav_iono.beta0; - nav_data.ion_cmp[7] = beidou_dnav_iono.beta3; + d_nav_data.ion_cmp[0] = beidou_dnav_iono.alpha0; + d_nav_data.ion_cmp[1] = beidou_dnav_iono.alpha1; + d_nav_data.ion_cmp[2] = beidou_dnav_iono.alpha2; + d_nav_data.ion_cmp[3] = beidou_dnav_iono.alpha3; + d_nav_data.ion_cmp[4] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[5] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[6] = beidou_dnav_iono.beta0; + d_nav_data.ion_cmp[7] = beidou_dnav_iono.beta3; } if (gps_utc_model.valid) { - nav_data.utc_gps[0] = gps_utc_model.A0; - nav_data.utc_gps[1] = gps_utc_model.A1; - nav_data.utc_gps[2] = gps_utc_model.tot; - nav_data.utc_gps[3] = gps_utc_model.WN_T; - nav_data.leaps = gps_utc_model.DeltaT_LS; + d_nav_data.utc_gps[0] = gps_utc_model.A0; + d_nav_data.utc_gps[1] = gps_utc_model.A1; + d_nav_data.utc_gps[2] = gps_utc_model.tot; + d_nav_data.utc_gps[3] = gps_utc_model.WN_T; + d_nav_data.leaps = gps_utc_model.DeltaT_LS; } if (!(gps_utc_model.valid) and gps_cnav_utc_model.valid) { - nav_data.utc_gps[0] = gps_cnav_utc_model.A0; - nav_data.utc_gps[1] = gps_cnav_utc_model.A1; - nav_data.utc_gps[2] = gps_cnav_utc_model.tot; - nav_data.utc_gps[3] = gps_cnav_utc_model.WN_T; - nav_data.leaps = gps_cnav_utc_model.DeltaT_LS; + d_nav_data.utc_gps[0] = gps_cnav_utc_model.A0; + d_nav_data.utc_gps[1] = gps_cnav_utc_model.A1; + d_nav_data.utc_gps[2] = gps_cnav_utc_model.tot; + d_nav_data.utc_gps[3] = gps_cnav_utc_model.WN_T; + d_nav_data.leaps = gps_cnav_utc_model.DeltaT_LS; } if (glonass_gnav_utc_model.valid) { - nav_data.utc_glo[0] = glonass_gnav_utc_model.d_tau_c; // ?? - nav_data.utc_glo[1] = 0.0; // ?? - nav_data.utc_glo[2] = 0.0; // ?? - nav_data.utc_glo[3] = 0.0; // ?? + d_nav_data.utc_glo[0] = glonass_gnav_utc_model.d_tau_c; // ?? + d_nav_data.utc_glo[1] = 0.0; // ?? + d_nav_data.utc_glo[2] = 0.0; // ?? + d_nav_data.utc_glo[3] = 0.0; // ?? } if (galileo_utc_model.A0 != 0.0) { - nav_data.utc_gal[0] = galileo_utc_model.A0; - nav_data.utc_gal[1] = galileo_utc_model.A1; - nav_data.utc_gal[2] = galileo_utc_model.tot; - nav_data.utc_gal[3] = galileo_utc_model.WNot; - nav_data.leaps = galileo_utc_model.Delta_tLS; + d_nav_data.utc_gal[0] = galileo_utc_model.A0; + d_nav_data.utc_gal[1] = galileo_utc_model.A1; + d_nav_data.utc_gal[2] = galileo_utc_model.tot; + d_nav_data.utc_gal[3] = galileo_utc_model.WNot; + d_nav_data.leaps = galileo_utc_model.Delta_tLS; } if (beidou_dnav_utc_model.valid) { - nav_data.utc_cmp[0] = beidou_dnav_utc_model.A0_UTC; - nav_data.utc_cmp[1] = beidou_dnav_utc_model.A1_UTC; - nav_data.utc_cmp[2] = 0.0; // ?? - nav_data.utc_cmp[3] = 0.0; // ?? - nav_data.leaps = beidou_dnav_utc_model.DeltaT_LS; + d_nav_data.utc_cmp[0] = beidou_dnav_utc_model.A0_UTC; + d_nav_data.utc_cmp[1] = beidou_dnav_utc_model.A1_UTC; + d_nav_data.utc_cmp[2] = 0.0; // ?? + d_nav_data.utc_cmp[3] = 0.0; // ?? + d_nav_data.leaps = beidou_dnav_utc_model.DeltaT_LS; } /* update carrier wave length using native function call in RTKlib */ @@ -990,11 +990,11 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ { for (int j = 0; j < NFREQ; j++) { - nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &nav_data); + d_nav_data.lam[i][j] = satwavelen(i + 1, d_rtklib_freq_index[j], &d_nav_data); } } - result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &nav_data); + result = rtkpos(&d_rtk, d_obs_data.data(), valid_obs + glo_valid_obs, &d_nav_data); if (result == 0) { @@ -1095,7 +1095,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ // TOW d_monitor_pvt.TOW_at_current_symbol_ms = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; // WEEK - d_monitor_pvt.week = adjgpsweek(nav_data.eph[0].week, this->is_pre_2009()); + d_monitor_pvt.week = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); // PVT GPS time d_monitor_pvt.RX_time = gnss_observables_map.cbegin()->second.RX_time; // User clock offset [s] @@ -1161,7 +1161,7 @@ bool Rtklib_Solver::get_PVT(const std::map &gnss_observables_ tmp_uint32 = gnss_observables_map.cbegin()->second.TOW_at_current_symbol_ms; d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // WEEK - tmp_uint32 = adjgpsweek(nav_data.eph[0].week, this->is_pre_2009()); + tmp_uint32 = adjgpsweek(d_nav_data.eph[0].week, this->is_pre_2009()); d_dump_file.write(reinterpret_cast(&tmp_uint32), sizeof(uint32_t)); // PVT GPS time tmp_double = gnss_observables_map.cbegin()->second.RX_time; diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 697e46687..4fbb66bda 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -132,6 +132,7 @@ private: uint32_t d_type_of_rx; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; + nav_t d_nav_data; }; diff --git a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc index 7d821f21b..6b095bdcd 100644 --- a/src/algorithms/libs/rtklib/rtklib_ephemeris.cc +++ b/src/algorithms/libs/rtklib/rtklib_ephemeris.cc @@ -34,6 +34,7 @@ #include "rtklib_preceph.h" #include "rtklib_rtkcmn.h" #include "rtklib_sbas.h" +#include /* constants -----------------------------------------------------------------*/ @@ -1023,7 +1024,7 @@ int satpos(gtime_t time, gtime_t teph, int sat, int ephopt, void satposs(gtime_t teph, const obsd_t *obs, int n, const nav_t *nav, int ephopt, double *rs, double *dts, double *var, int *svh) { - gtime_t time[MAXOBS] = {}; + std::vector time(MAXOBS); double dt; double pr; int i; diff --git a/src/algorithms/libs/rtklib/rtklib_ionex.cc b/src/algorithms/libs/rtklib/rtklib_ionex.cc index 4bce957ea..418782557 100644 --- a/src/algorithms/libs/rtklib/rtklib_ionex.cc +++ b/src/algorithms/libs/rtklib/rtklib_ionex.cc @@ -39,6 +39,7 @@ #include "rtklib_ionex.h" #include "rtklib_rtkcmn.h" #include +#include /* get index -----------------------------------------------------------------*/ int getindex(double value, const double *range) @@ -425,8 +426,8 @@ void readtec(const char *file, nav_t *nav, int opt) double hgts[3] = {0}; double rb = 0.0; double nexp = -1.0; - double dcb[MAXSAT] = {0}; - double rms[MAXSAT] = {0}; + std::vector dcb(MAXSAT, 0); + std::vector rms(MAXSAT, 0); int i; int n; char *efiles[MAXEXFILE]; @@ -463,7 +464,7 @@ void readtec(const char *file, nav_t *nav, int opt) } /* read ionex header */ - if (readionexh(fp, lats, lons, hgts, &rb, &nexp, dcb, rms) <= 0.0) + if (readionexh(fp, lats, lons, hgts, &rb, &nexp, dcb.data(), rms.data()) <= 0.0) { trace(2, "ionex file format error %s\n", efiles[i]); fclose(fp); diff --git a/src/algorithms/libs/rtklib/rtklib_pntpos.cc b/src/algorithms/libs/rtklib/rtklib_pntpos.cc index 5d023842b..abc3c2579 100644 --- a/src/algorithms/libs/rtklib/rtklib_pntpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_pntpos.cc @@ -34,6 +34,7 @@ #include "rtklib_ionex.h" #include "rtklib_sbas.h" #include +#include /* pseudorange measurement error variance ------------------------------------*/ double varerr(const prcopt_t *opt, double el, int sys) @@ -988,8 +989,8 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, double *resp; int i; int stat; - int vsat[MAXOBS] = {0}; - int svh[MAXOBS]; + std::vector vsat(MAXOBS, 0); + std::vector svh(MAXOBS, 0); sol->stat = SOLQ_NONE; @@ -1019,20 +1020,20 @@ int pntpos(const obsd_t *obs, int n, const nav_t *nav, opt_.tropopt = TROPOPT_SAAS; } /* satellite positions, velocities and clocks */ - satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh); + satposs(sol->time, obs, n, nav, opt_.sateph, rs, dts, var, svh.data()); /* estimate receiver position with pseudorange */ - stat = estpos(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + stat = estpos(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg); /* raim fde */ if (!stat && n >= 6 && opt->posopt[4]) { - stat = raim_fde(obs, n, rs, dts, var, svh, nav, &opt_, sol, azel_, vsat, resp, msg); + stat = raim_fde(obs, n, rs, dts, var, svh.data(), nav, &opt_, sol, azel_, vsat.data(), resp, msg); } /* estimate receiver velocity with doppler */ if (stat) { - estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat); + estvel(obs, n, rs, dts, nav, &opt_, sol, azel_, vsat.data()); } if (azel) diff --git a/src/algorithms/libs/rtklib/rtklib_ppp.cc b/src/algorithms/libs/rtklib/rtklib_ppp.cc index 84d1689be..5e4280f41 100644 --- a/src/algorithms/libs/rtklib/rtklib_ppp.cc +++ b/src/algorithms/libs/rtklib/rtklib_ppp.cc @@ -38,6 +38,7 @@ #include "rtklib_sbas.h" #include "rtklib_tides.h" #include +#include /* wave length of LC (m) -----------------------------------------------------*/ double lam_LC(int i, int j, int k) @@ -351,7 +352,7 @@ int sel_amb(int *sat1, int *sat2, double *N, double *var, int n) { int i; int j; - int flgs[MAXSAT] = {0}; + std::vector flgs(MAXSAT, 0); int max_flg = 0; /* sort by variance */ @@ -372,7 +373,7 @@ int sel_amb(int *sat1, int *sat2, double *N, double *var, int n) /* select linearly independent satellite pair */ for (i = j = 0; i < n; i++) { - if (!is_depend(sat1[i], sat2[i], flgs, &max_flg)) + if (!is_depend(sat1[i], sat2[i], flgs.data(), &max_flg)) { continue; } @@ -546,7 +547,7 @@ int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n) int m = 0; int info; int stat; - int flgs[MAXSAT] = {0}; + std::vector flgs(MAXSAT, 0); int max_flg = 0; lam1 = LAM_CARR[0]; @@ -565,7 +566,7 @@ int fix_amb_ILS(rtk_t *rtk, int *sat1, int *sat2, int *NW, int n) for (i = 0; i < n; i++) { /* check linear independency */ - if (!is_depend(sat1[i], sat2[i], flgs, &max_flg)) + if (!is_depend(sat1[i], sat2[i], flgs.data(), &max_flg)) { continue; } @@ -1362,7 +1363,7 @@ void udbias_ppp(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) { double meas[2]; double var[2]; - double bias[MAXOBS] = {0}; + std::vector bias(MAXOBS, 0.0); double offset = 0.0; double pos[3] = {0}; int i; @@ -1562,7 +1563,7 @@ int res_ppp(int iter __attribute__((unused)), const obsd_t *obs, int n, const do double dtdx[3]; double dantr[NFREQ] = {0}; double dants[NFREQ] = {0}; - double var[MAXOBS * 2]; + std::vector var(MAXOBS * 2, 0.0); double dtrp = 0.0; double vart = 0.0; double varm[2] = {0}; @@ -1781,7 +1782,7 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) int i; int nv; int info; - int svh[MAXOBS]; + std::vector svh(MAXOBS); int stat = SOLQ_SINGLE; trace(3, "pppos : nx=%d n=%d\n", rtk->nx, n); @@ -1803,7 +1804,7 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) tracemat(4, rtk->x, 1, NR_PPP(opt), 13, 4); /* satellite positions and clocks */ - satposs(obs[0].time, obs, n, nav, rtk->opt.sateph, rs, dts, var, svh); + satposs(obs[0].time, obs, n, nav, rtk->opt.sateph, rs, dts, var, svh.data()); /* exclude measurements of eclipsing satellite */ if (rtk->opt.posopt[3]) @@ -1821,7 +1822,7 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) for (i = 0; i < rtk->opt.niter; i++) { /* phase and code residuals */ - if ((nv = res_ppp(i, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel)) <= 0) + if ((nv = res_ppp(i, obs, n, rs, dts, var, svh.data(), nav, xp, rtk, v, H, R, azel)) <= 0) { break; } @@ -1842,7 +1843,7 @@ void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav) if (stat == SOLQ_PPP) { /* postfit residuals */ - res_ppp(1, obs, n, rs, dts, var, svh, nav, xp, rtk, v, H, R, azel); + res_ppp(1, obs, n, rs, dts, var, svh.data(), nav, xp, rtk, v, H, R, azel); /* update state and covariance matrix */ matcpy(rtk->x, xp, rtk->nx, 1); diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc index 5d297fe44..9c09dd392 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.cc +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -43,6 +43,7 @@ #include "rtklib_preceph.h" #include "rtklib_rtkcmn.h" #include +#include /* satellite code to satellite system ----------------------------------------*/ int code2sys(char code) @@ -390,7 +391,7 @@ void readsp3(const char *file, nav_t *nav, int opt) int j; int n; int ns; - int sats[MAXSAT] = {}; + std::vector sats(MAXSAT); char *efiles[MAXEXFILE]; char *ext; char type = ' '; @@ -431,10 +432,10 @@ void readsp3(const char *file, nav_t *nav, int opt) continue; } /* read sp3 header */ - ns = readsp3h(fp, &time, &type, sats, bfact, tsys); + ns = readsp3h(fp, &time, &type, sats.data(), bfact, tsys); /* read sp3 body */ - readsp3b(fp, type, sats, ns, bfact, tsys, j++, opt, nav); + readsp3b(fp, type, sats.data(), ns, bfact, tsys, j++, opt, nav); fclose(fp); } diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 53c1c47cd..81842d48a 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -31,6 +31,7 @@ #include "rtklib_rtkcmn.h" #include +#include #include #include #include @@ -40,6 +41,7 @@ #include #include #include +#include const double GPST0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ @@ -4496,7 +4498,7 @@ double satazel(const double *pos, const double *e, double *azel) *-----------------------------------------------------------------------------*/ void dops(int ns, const double *azel, double elmin, double *dop) { - double H[4 * MAXSAT]; + std::vector H(4 * MAXSAT); double Q[16]; double cosel; double sinel; @@ -4525,7 +4527,7 @@ void dops(int ns, const double *azel, double elmin, double *dop) return; } - matmul("NT", 4, 4, n, 1.0, H, H, 0.0, Q); + matmul("NT", 4, 4, n, 1.0, H.data(), H.data(), 0.0, Q); if (!matinv(Q, 4)) { dop[0] = std::sqrt(Q[0] + Q[5] + Q[10] + Q[15]); /* GDOP */ @@ -5029,16 +5031,23 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun, *-----------------------------------------------------------------------------*/ void csmooth(obs_t *obs, int ns) { - double Ps[2][MAXSAT][NFREQ] = {}; - double Lp[2][MAXSAT][NFREQ] = {}; + std::vector> Ps[2]; + std::vector> Lp[2]; double dcp; int i; int j; int s; int r; - int n[2][MAXSAT][NFREQ] = {}; + std::vector> n[2]; obsd_t *p; + Ps[0].resize(MAXSAT); + Ps[1].resize(MAXSAT); + Lp[0].resize(MAXSAT); + Lp[1].resize(MAXSAT); + n[0].resize(MAXSAT); + n[1].resize(MAXSAT); + trace(3, "csmooth: nobs=%d,ns=%d\n", obs->n, ns); for (i = 0; i < obs->n; i++) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 490321f2a..54bcaa802 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -37,6 +37,7 @@ #include "rtklib_tides.h" #include #include +#include static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)), const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute__((unused)), const nav_t *nav __attribute((unused)), @@ -2029,7 +2030,7 @@ void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), doubl int n; int m; int f; - int index[MAXSAT]; + std::vector index(MAXSAT); int nv = 0; int nf = NF_RTK(&rtk->opt); @@ -2083,7 +2084,7 @@ void holdamb(rtk_t *rtk, const double *xa) int m; int f; int info; - int index[MAXSAT]; + std::vector index(MAXSAT); int nb = rtk->nx - rtk->na; int nv = 0; int nf = NF_RTK(&rtk->opt); @@ -2371,13 +2372,13 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, int ns; int ny; int nv; - int sat[MAXSAT]; - int iu[MAXSAT]; - int ir[MAXSAT]; + std::vector sat(MAXSAT); + std::vector iu(MAXSAT); + std::vector ir(MAXSAT); int niter; int info; - int vflg[MAXOBS * NFREQ * 2 + 1]; - int svh[MAXOBS * 2]; + std::vector vflg(MAXOBS * NFREQ * 2 + 1); + std::vector svh(MAXOBS * 2); int stat = rtk->opt.mode <= PMODE_DGPS ? SOLQ_DGPS : SOLQ_FLOAT; int nf = opt->ionoopt == IONOOPT_IFLC ? 1 : opt->nf; @@ -2401,10 +2402,10 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, } } /* satellite positions/clocks */ - satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh); + satposs(time, obs, n, nav, opt->sateph, rs, dts, var, svh.data()); /* undifferenced residuals for base station */ - if (!zdres(1, obs + nu, nr, rs + nu * 6, dts + nu * 2, svh + nu, nav, rtk->rb, opt, 1, + if (!zdres(1, obs + nu, nr, rs + nu * 6, dts + nu * 2, &svh[nu], nav, rtk->rb, opt, 1, y + nu * nf * 2, e + nu * 3, azel + nu * 2)) { errmsg(rtk, "initial base station position error\n"); @@ -2423,7 +2424,7 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, dt = intpres(time, obs + nu, nr, nav, rtk, y + nu * nf * 2); } /* select common satellites between rover and base-station */ - if ((ns = selsat(obs, azel, nu, nr, opt, sat, iu, ir)) <= 0) + if ((ns = selsat(obs, azel, nu, nr, opt, sat.data(), iu.data(), ir.data())) <= 0) { errmsg(rtk, "no common satellite\n"); @@ -2436,7 +2437,7 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, return 0; } /* temporal update of states */ - udstate(rtk, obs, sat, iu, ir, ns, nav); + udstate(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav); trace(4, "x(0)="); tracemat(4, rtk->x, 1, NR_RTK(opt), 13, 4); @@ -2458,14 +2459,14 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, for (i = 0; i < niter; i++) { /* undifferenced residuals for rover */ - if (!zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel)) + if (!zdres(0, obs, nu, rs, dts, svh.data(), nav, xp, opt, 0, y, e, azel)) { errmsg(rtk, "rover initial position error\n"); stat = SOLQ_NONE; break; } /* double-differenced residuals and partial derivatives */ - if ((nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, H, R, vflg)) < 1) + if ((nv = ddres(rtk, nav, dt, xp, Pp, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, H, R, vflg.data())) < 1) { errmsg(rtk, "no double-differenced residual\n"); stat = SOLQ_NONE; @@ -2482,13 +2483,13 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, trace(4, "x(%d)=", i + 1); tracemat(4, xp, 1, NR_RTK(opt), 13, 4); } - if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel)) + if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh.data(), nav, xp, opt, 0, y, e, azel)) { /* post-fit residuals for float solution */ - nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, nullptr, R, vflg); + nv = ddres(rtk, nav, dt, xp, Pp, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, nullptr, R, vflg.data()); /* validation of float solution */ - if (valpos(rtk, v, R, vflg, nv, 4.0)) + if (valpos(rtk, v, R, vflg.data(), nv, 4.0)) { /* update state and covariance matrix */ matcpy(rtk->x, xp, rtk->nx, 1); @@ -2526,7 +2527,7 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, /* resolve integer ambiguity by WL-NL */ if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_WLNL) { - if (resamb_WLNL(rtk, obs, sat, iu, ir, ns, nav, azel)) + if (resamb_WLNL(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav, azel)) { stat = SOLQ_FIX; } @@ -2534,7 +2535,7 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, /* resolve integer ambiguity by TCAR */ else if (stat != SOLQ_NONE && rtk->opt.modear == ARMODE_TCAR) { - if (resamb_TCAR(rtk, obs, sat, iu, ir, ns, nav, azel)) + if (resamb_TCAR(rtk, obs, sat.data(), iu.data(), ir.data(), ns, nav, azel)) { stat = SOLQ_FIX; } @@ -2542,13 +2543,13 @@ int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, /* resolve integer ambiguity by LAMBDA */ else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa) > 1) { - if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel)) + if (zdres(0, obs, nu, rs, dts, svh.data(), nav, xa, opt, 0, y, e, azel)) { /* post-fit reisiduals for fixed solution */ - nv = ddres(rtk, nav, dt, xa, nullptr, sat, y, e, azel, iu, ir, ns, v, nullptr, R, vflg); + nv = ddres(rtk, nav, dt, xa, nullptr, sat.data(), y, e, azel, iu.data(), ir.data(), ns, v, nullptr, R, vflg.data()); /* validation of fixed solution */ - if (valpos(rtk, v, R, vflg, nv, 4.0)) + if (valpos(rtk, v, R, vflg.data(), nv, 4.0)) { /* hold integer ambiguity */ if (++rtk->nfix >= rtk->opt.minfix && diff --git a/src/algorithms/libs/rtklib/rtklib_solution.cc b/src/algorithms/libs/rtklib/rtklib_solution.cc index 57aa8e26e..17ad78ead 100644 --- a/src/algorithms/libs/rtklib/rtklib_solution.cc +++ b/src/algorithms/libs/rtklib/rtklib_solution.cc @@ -37,6 +37,7 @@ #include #include #include +#include /* constants and macros ------------------------------------------------------*/ @@ -1654,7 +1655,7 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, int sat; int sys; int nsat; - int prn[MAXSAT]; + std::vector prn(MAXSAT); char *p = reinterpret_cast(buff); char *q; char *s; @@ -1683,7 +1684,7 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, { continue; } - sys = satsys(sat, prn + nsat); + sys = satsys(sat, &prn[nsat]); if (sys != SYS_GPS && sys != SYS_SBS) { continue; @@ -1728,7 +1729,7 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, { continue; } - if (satsys(sat, prn + nsat) != SYS_GLO) + if (satsys(sat, &prn[nsat]) != SYS_GLO) { continue; } @@ -1769,7 +1770,7 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, { continue; } - if (satsys(sat, prn + nsat) != SYS_GAL) + if (satsys(sat, &prn[nsat]) != SYS_GAL) { continue; } @@ -1809,7 +1810,7 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, { continue; } - if (satsys(sat, prn + nsat) != SYS_BDS) + if (satsys(sat, &prn[nsat]) != SYS_BDS) { continue; } @@ -1861,7 +1862,7 @@ int outnmea_gsv(unsigned char *buff, const sol_t *sol, int prn; int sys; int nmsg; - int sats[MAXSAT]; + std::vector sats(MAXSAT); char *p = reinterpret_cast(buff); char *q; char *s; From 79d516d4d41ea34905532143364d435e2716c238 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 15 Jul 2022 13:02:10 +0200 Subject: [PATCH 4/5] Improve code visualization in some editors --- src/algorithms/libs/beidou_b1i_signal_replica.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/libs/beidou_b1i_signal_replica.cc b/src/algorithms/libs/beidou_b1i_signal_replica.cc index 88e22c78b..ce52bbc8c 100644 --- a/src/algorithms/libs/beidou_b1i_signal_replica.cc +++ b/src/algorithms/libs/beidou_b1i_signal_replica.cc @@ -73,7 +73,7 @@ void beidou_b1i_code_gen_int(own::span dest, int32_t prn, uint32_t chip } // Set the delay - delay = code_length; //********************************** + delay = code_length; // ********************************* delay += chip_shift; delay %= code_length; From eeaf7fb73e047b4da8754682ae22ed90613feb93 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 15 Jul 2022 16:59:03 +0200 Subject: [PATCH 5/5] Reorder private members --- src/algorithms/PVT/libs/rtklib_solver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/algorithms/PVT/libs/rtklib_solver.h b/src/algorithms/PVT/libs/rtklib_solver.h index 4fbb66bda..2e7fc8010 100644 --- a/src/algorithms/PVT/libs/rtklib_solver.h +++ b/src/algorithms/PVT/libs/rtklib_solver.h @@ -128,11 +128,11 @@ private: std::string d_dump_filename; std::ofstream d_dump_file; rtk_t d_rtk{}; + nav_t d_nav_data{}; Monitor_Pvt d_monitor_pvt{}; uint32_t d_type_of_rx; bool d_flag_dump_enabled; bool d_flag_dump_mat_enabled; - nav_t d_nav_data; };