From 3a9c4c71f76f30b3ddd091fbf6ccaf3ee012ec37 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sun, 20 Jun 2021 23:58:36 +0900 Subject: [PATCH 01/59] Implement GLONASS ICD definition to GLONASS.h --- tool/navigation/GLONASS.h | 524 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 524 insertions(+) create mode 100644 tool/navigation/GLONASS.h diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h new file mode 100644 index 000000000..ba1a52829 --- /dev/null +++ b/tool/navigation/GLONASS.h @@ -0,0 +1,524 @@ +/* + * Copyright (c) 2016, M.Naruoka (fenrir) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of the naruoka.org nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef __GLONASS_H__ +#define __GLONASS_H__ + +/** @file + * @brief GLONASS ICD definitions + */ + + +#include +#define _USE_MATH_DEFINES +#include +#include +#include + +#include "GPS.h" + +template +class GLONASS_SpaceNode { + public: + typedef FloatT float_t; + + public: + typedef GLONASS_SpaceNode self_t; + + typedef unsigned char u8_t; + typedef signed char s8_t; + typedef unsigned short u16_t; + typedef signed short s16_t; + typedef unsigned int u32_t; + typedef signed int s32_t; + + typedef int int_t; + typedef unsigned int uint_t; + + typedef typename GPS_SpaceNode::DataParser DataParser; + + template + struct BroadcastedMessage : public DataParser { +#define convert_u(bits, offset_bits, length, name) \ +static u ## bits ## _t name(const InputT *buf){ \ + return \ + DataParser::template bits2num( \ + buf, offset_bits, length); \ +} +#define convert_s(bits, offset_bits, length, name) \ +static s ## bits ## _t name(const InputT *buf){ \ + u ## bits ## _t temp( \ + DataParser::template bits2num( \ + buf, offset_bits, length)); \ + static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); /* MSB is sign bit */ \ + return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \ +} + convert_u( 8, 1, 4, m); + convert_u( 8, 77, 8, KX); + + struct String1 { + convert_u( 8, 7, 2, P1); + convert_u(16, 9, 12, t_k); + convert_s(32, 21, 24, xn_dot); + convert_s( 8, 45, 5, xn_ddot); + convert_s(32, 50, 27, xn); + }; + struct String2 { + convert_u( 8, 5, 3, B_n); + convert_u( 8, 8, 1, P2); + convert_u( 8, 9, 7, t_b); + convert_s(32, 21, 24, yn_dot); + convert_s( 8, 45, 5, yn_ddot); + convert_s(32, 50, 27, yn); + }; + struct String3 { + convert_u( 8, 5, 1, P3); + convert_s(16, 6, 11, gamma_n); + convert_u( 8, 18, 2, p); + convert_u( 8, 20, 1, l_n); + convert_s(32, 21, 24, zn_dot); + convert_s( 8, 45, 5, zn_ddot); + convert_s(32, 50, 27, zn); + }; + struct String4 { + convert_s(32, 5, 22, tau_n); + convert_u( 8, 27, 5, delta_tau_n); + convert_u( 8, 32, 5, E_n); + convert_u( 8, 51, 1, P4); + convert_u( 8, 52, 4, F_T); + convert_u(16, 59, 11, N_T); + convert_u( 8, 70, 5, n); + convert_u( 8, 75, 2, M); + }; + struct String5_Alnamac { + convert_u(16, 5, 11, NA); + convert_s(32, 16, 32, tau_c); + convert_u( 8, 49, 5, N_4); + convert_s(32, 54, 22, tau_GPS); + convert_u( 8, 76, 1, l_n); + }; + + struct String6_Alnamac { // String 6; same as 8, 10, 12, 14 + convert_u( 8, 5, 1, C_n); + convert_u( 8, 6, 2, M_n); + convert_u( 8, 8, 5, nA); + convert_s(16, 13, 10, tauA_n); + convert_s(32, 23, 21, lambdaA_n); + convert_s(32, 44, 18, delta_iA_n); + convert_u(16, 62, 15, epsilonA_n); + }; + struct String7_Alnamac { // String 7; same as 9, 11, 13, 15 + convert_s(16, 5, 16, omegaA_n); + convert_u(32, 21, 21, tA_lambda_n); + convert_s(32, 42, 22, delta_TA_n); + convert_s( 8, 64, 7, delta_TA_dot_n); + convert_u( 8, 71, 5, HA_n); + convert_u( 8, 76, 1, l_n); + }; + + struct Frame5_String14 { + convert_s(16, 5, 11, B1); + convert_s(16, 16, 10, B2); + convert_u( 8, 26, 2, KP); + }; +#undef convert_s +#undef convert_u + }; + + struct TimeProperties { + float_t tau_c; // [s] + float_t tau_GPS; // [s] + struct date_t { + uint_t year; // 20XX + uint_t day_of_year; // Jan. 1st = 0 + } date; + bool l_n; + + struct raw_t { + s32_t tau_c; + s32_t tau_GPS; + u8_t N_4; + u16_t NA; + bool l_n; +#define fetch_item(name) name = BroadcastedMessage< \ + InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \ + :: String5_Alnamac :: name (src) + template + void update_string5(const InputT *src){ + fetch_item(tau_c); + fetch_item(tau_GPS); + fetch_item(N_4); + fetch_item(NA); + fetch_item(l_n); + } +#undef fetch_item + + enum { + SF_tau_c, SF_tau_GPS, + SF_NUM, + }; + static const float_t sf[SF_NUM]; + + static date_t raw2date(const u8_t &N_4_, const u16_t &NA_) { + // @see A.3.1.3 (ver.5.1) + date_t res; + res.year = 1996 + 4 * (N_4_ - 1); + if(NA_ <= 366){ + res.day_of_year = NA_ - 1; + }else if(NA_ <= 731){ + res.year += 1; + res.day_of_year = NA_ - 367; + }else if(NA_ <= 1096){ + res.year += 2; + res.day_of_year = NA_ - 732; + }else if(NA_ <= 1461){ + res.year += 3; + res.day_of_year = NA_ - 1097; + } + return res; + } + + operator TimeProperties() const { + TimeProperties res; +#define CONVERT(TARGET) \ +{res.TARGET = sf[SF_ ## TARGET] * TARGET;} + CONVERT(tau_c); + CONVERT(tau_GPS); + res.date = raw2date(N_4, NA); + res.l_n = (l_n > 0); +#undef CONVERT + return res; + } + + raw_t &operator=(const TimeProperties &t){ +#define CONVERT(TARGET) \ +{TARGET = (s32_t)((t.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);} + CONVERT(tau_c); + CONVERT(tau_GPS); + std::div_t divmod(std::div(t.date.year - 1996, 4)); + N_4 = divmod.quot + 1; + NA = t.date.day_of_year + 1; + switch(divmod.rem){ + case 1: NA += 366; break; + case 2: NA += 731; break; + case 3: NA += 1096; break; + } + l_n = (t.l_n ? 1 : 0); +#undef CONVERT + return *this; + } + }; + }; + + class SatelliteProperties { + public: + struct Ephemeris { + uint_t t_k; // time base [s] + uint_t t_b; // time interval to UTC(SU) + 3hr [s] + uint_t M; // satellite type; 0 - GLONASS, 1 - GLONASS-M + float_t gamma_n; + float_t tau_n; // [s] + float_t xn, yn, zn; // [m] + float_t xn_dot, yn_dot, zn_dot; // [m/s] + float_t xn_ddot, yn_ddot, zn_ddot; // [m/s^2] + uint_t B_n; // health flag + uint_t p; // satellite operation mode + uint_t N_T; // current date counting up from leap year Jan. 1st [days] + float_t F_T; // user range accuracy at t_b [m] + uint_t n; // time difference between L1 and L2 + float_t delta_tau_n; + uint_t E_n; // [days] + uint_t P1; // [s] time interval of two adjacent t_b; 0 - 0, 1 - 30, 2 - 45, 3 - 60 mins + bool P2; + //bool P3; // flag for alnamac; 1 - five, 0 - four + bool P4; // flag for ephemeris; 1 - uploaded by the control segment + bool l_n; // health flag; 0 - healthy, 1 - malfunction + + u8_t F_T_index() const; + + struct raw_t { + // String1 + u8_t P1; + u16_t t_k; + s32_t xn_dot; ///< SF: 2^-20 [km/s] + s8_t xn_ddot; ///< SF: 2^-30 [km/s^2] + s32_t xn; ///< SF: 2^-11 [km] + + // String2 + u8_t B_n; + u8_t P2; + u8_t t_b; ///< SF: 15 + s32_t yn_dot; + s8_t yn_ddot; + s32_t yn; + + // String3 + u8_t P3; + s16_t gamma_n; ///< SF: 2^-40 + u8_t p; + u8_t l_n; + s32_t zn_dot; + s8_t zn_ddot; + s32_t zn; + + // String4 + s32_t tau_n; ///< SF: 2^-30 + u8_t delta_tau_n; ///< SF: 2^-30 + u8_t E_n; + u8_t P4; + u8_t F_T; + u16_t N_T; ///< [days] + u8_t n; + u8_t M; + +#define fetch_item(num, name) name = BroadcastedMessage< \ + InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \ + :: String ## num :: name (src) + template + void update_string1(const InputT *src){ + fetch_item(1, P1); + fetch_item(1, t_k); + fetch_item(1, xn_dot); + fetch_item(1, xn_ddot); + fetch_item(1, xn); + } + template + void update_string2(const InputT *src){ + // String2 + fetch_item(2, B_n); + fetch_item(2, P2); + fetch_item(2, t_b); + fetch_item(2, yn_dot); + fetch_item(2, yn_ddot); + fetch_item(2, yn); + } + template + void update_string3(const InputT *src){ + fetch_item(3, P3); + fetch_item(3, gamma_n); + fetch_item(3, p); + fetch_item(3, l_n); + fetch_item(3, zn_dot); + fetch_item(3, zn_ddot); + fetch_item(3, zn); + } + template + void update_string4(const InputT *src){ + fetch_item(4, tau_n); + fetch_item(4, delta_tau_n); + fetch_item(4, E_n); + fetch_item(4, P4); + fetch_item(4, F_T); + fetch_item(4, N_T); + fetch_item(4, n); + fetch_item(4, M); + } +#undef fetch_item + + enum { + SF_xn, SF_yn = SF_xn, SF_zn = SF_xn, + SF_xn_dot, SF_yn_dot = SF_xn_dot, SF_zn_dot = SF_xn_dot, + SF_xn_ddot, SF_yn_ddot = SF_xn_ddot, SF_zn_ddot = SF_xn_ddot, + SF_t_b, + SF_gamma_n, + SF_tau_n, + SF_delta_tau_n, + SF_NUM, + }; + static const float_t sf[SF_NUM]; + + static const float_t F_T_table[15]; ///< @see Table 4.4 + + operator Ephemeris() const { + Ephemeris res; +#define CONVERT(TARGET) \ +{res.TARGET = sf[SF_ ## TARGET] * TARGET;} + res.t_k = (((t_k >> 7) & 0x1F) * 3600) // hour + + (((t_k >> 1) & 0x3F) * 60) // min + + ((t_k & 0x1) ? 30 : 0); // sec + CONVERT(t_b); + res.M = M; + CONVERT(gamma_n); + CONVERT(tau_n); + CONVERT(xn); CONVERT(xn_dot); CONVERT(xn_ddot); + CONVERT(yn); CONVERT(yn_dot); CONVERT(yn_ddot); + CONVERT(zn); CONVERT(zn_dot); CONVERT(zn_ddot); + res.B_n = B_n; + res.p = p; + res.N_T = N_T; + + if(F_T >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){ + res.F_T = -1; // not used + }else{ + res.F_T = F_T_table[F_T]; + } + res.n = n; + CONVERT(delta_tau_n); + res.E_n = E_n; + switch(P1){ + case 1: res.P1 = 30 * 60; break; + case 2: res.P1 = 45 * 60; break; + case 3: res.P1 = 60 * 60; break; + case 0: default: res.P1 = 0; break; + } + res.P2 = (P2 > 0); + //res.P3 = (P3 > 0); + res.P4 = (P4 > 0); + res.l_n = (l_n > 0); +#undef CONVERT + return res; + } + raw_t &operator=(const Ephemeris &eph){ + // TODO: m? +#define CONVERT(TARGET) \ +{TARGET = (s32_t)((eph.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);} + { // t_k + std::div_t minutes(div(eph.t_k, 60)); + std::div_t hrs(div(minutes.quot, 60)); + t_k = (((hrs.quot << 6) + minutes.quot) << 1) + (minutes.rem > 0 ? 1 : 0); + } + CONVERT(t_b); + M = eph.M; + CONVERT(gamma_n); + CONVERT(tau_n); + CONVERT(xn); CONVERT(xn_dot); CONVERT(xn_ddot); + CONVERT(yn); CONVERT(yn_dot); CONVERT(yn_ddot); + CONVERT(zn); CONVERT(zn_dot); CONVERT(zn_ddot); + B_n = eph.B_n; + p = eph.p; + N_T = eph.N_T; + F_T = eph.F_T_index(); + n = eph.n; + CONVERT(delta_tau_n); + E_n = eph.E_n; + if(eph.P1 > 45 * 60){ + P1 = 3; + }else if(eph.P1 > 30 * 60){ + P1 = 2; + }else if(eph.P1 > 0){ + P1 = 1; + }else{ + P1 = 0; + } + P2 = (eph.P2 ? 1 : 0); + //P3 = (eph.P3 ? 1 : 0); + P4 = (eph.P4 ? 1 : 0); + l_n = (eph.l_n ? 1 : 0); +#undef CONVERT + return *this; + } + }; + + bool is_equivalent(const Ephemeris &eph) const { + do{ +#define CHECK(TARGET) if(TARGET != eph.TARGET){break;} + CHECK(t_k); + CHECK(t_b); + CHECK(M); + CHECK(B_n); + CHECK(p); + CHECK(N_T); + CHECK(F_T_index()); + CHECK(n); + CHECK(E_n); + CHECK(P1); + CHECK(P2); + //CHECK(P3); + CHECK(P4); + CHECK(l_n); +#undef CHECK +#define CHECK(TARGET) \ +if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} + CHECK(gamma_n); + CHECK(tau_n); + CHECK(xn); CHECK(xn_dot); CHECK(xn_ddot); + CHECK(yn); CHECK(yn_dot); CHECK(yn_ddot); + CHECK(zn); CHECK(zn_dot); CHECK(zn_ddot); + CHECK(delta_tau_n); +#undef CHECK + return true; + }while(false); + return false; + } + }; + }; + + struct Satellite : public SatelliteProperties { + + }; +}; + +#define POWER_2(n) \ +(((n) >= 0) \ + ? (float_t)(1 << (n >= 0 ? n : 0)) \ + : (((float_t)1) / (1 << (-(n) >= 30 ? 30 : -(n > 0 ? 0 : n))) \ + / (1 << (-(n) >= 30 ? (-(n) - 30) : 0)))) + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::TimeProperties::raw_t::sf[] = { + POWER_2(-31), // tau_c [s] + POWER_2(-30) * 60 * 60 * 24, // tau_GPS [day] => [s] +}; + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::raw_t::sf[] = { + POWER_2(-11) * 1E3, // x + POWER_2(-20) * 1E3, // x_dot + POWER_2(-30) * 1E3, // x_ddot + 15 * 60, // t_b + POWER_2(-40), // gamma_n + POWER_2(-30), // tau + POWER_2(-30), // delta_tau +}; + +#undef POWER_2 + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::raw_t::F_T_table[] = { + 1, 2, 2.5, 4, 5, 7, 10, 12, 14, 16, 32, 64, 128, 256, 512, +}; + +template +typename GLONASS_SpaceNode::u8_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::F_T_index() const { + if(F_T <= 0){ // invalid value + return sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]); + } + u8_t res(0); + while(res < (sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]))){ + if(F_T <= raw_t::F_T_table[res]){break;} + ++res; + } + return res; +} + +#endif /* __GLONASS_H__ */ From 6fd45b4df120e4c6ebd4cc96ef2efbc88963cb68 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 22 Jun 2021 09:07:30 +0900 Subject: [PATCH 02/59] Add GLONASS ephemeris parsing to GNSS_Data.h --- tool/INS_GPS/GNSS_Data.h | 55 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index cefd2d4ef..c3beb8a14 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -38,6 +38,7 @@ #include "SylphideProcessor.h" #include "navigation/GPS.h" +#include "navigation/GLONASS.h" template struct GNSS_Data { @@ -71,6 +72,15 @@ struct GNSS_Data { typedef typename gps_t::Ionospheric_UTC_Parameters gps_iono_utc_t; + typedef GLONASS_SpaceNode glonass_t; + typedef typename glonass_t::Satellite::Ephemeris glonass_ephemeris_t; + struct glonass_ephemeris_raw_t : public glonass_ephemeris_t::raw_t, glonass_t::TimeProperties::raw_t { + unsigned int super_frame; + unsigned int has_string; + glonass_ephemeris_raw_t() : has_string(0) {} + } glonass_ephemeris[24]; + + Loader() : gps(NULL) { for(unsigned int i(0); i < sizeof(gps_ephemeris) / sizeof(gps_ephemeris[0]); ++i){ gps_ephemeris[i].svid = i + 1; @@ -156,9 +166,54 @@ struct GNSS_Data { return false; } + bool load_glonass(const GNSS_Data &data){ + if(data.subframe.bytes != 16){return false;} + if((data.subframe.sv_number == 0) + || (data.subframe.sv_number > (sizeof(glonass_ephemeris) / sizeof(glonass_ephemeris[0])))){ + return false; // exclude unknown satellite, whose sv_number may be 255. + } + typename observer_t::u32_t *buf((typename observer_t::u32_t *)data.subframe.buffer); + typedef typename glonass_t::template BroadcastedMessage parser_t; + unsigned int super_frame(buf[3] >> 16), frame(buf[3] & 0xF), string_no(parser_t::m(buf)); + + if((string_no >= 1) && (string_no <= 5)){ // immediate info. (ephemeris) with time property (in non-immediate info,) + glonass_ephemeris_raw_t &eph(glonass_ephemeris[data.subframe.sv_number - 1]); + if((eph.has_string > 0) && (eph.super_frame != super_frame)){ + eph.has_string = 0; // clean up + } + eph.super_frame = super_frame; + eph.has_string |= (0x1 << (string_no - 1)); + switch(string_no){ // string num + case 1: eph.template update_string1<0, 0>(buf); break; + case 2: eph.template update_string2<0, 0>(buf); break; + case 3: eph.template update_string3<0, 0>(buf); break; + case 4: eph.template update_string4<0, 0>(buf); break; + case 5: { + eph.template update_string5<0, 0>(buf); + if(frame == 4){ + // TODO: require special care for 50th frame? @see Table 4.9 note (4) + } + break; + } + } + if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame + glonass_ephemeris_t eph_converted(eph); + typename glonass_t::TimeProperties t_prop(eph); + // TODO register ephemeris + eph.has_string = 0; + } + return true; + }else{ // non-immediate info. except for time info. (alnamac) + + } + return false; + } + bool load(const GNSS_Data &data){ switch(data.subframe.gnssID){ // TODO: other satellite systems + case observer_t::gnss_svid_t::GLONASS: + return load_glonass(data); case observer_t::gnss_svid_t::GPS: return load_gps(data); } From 17391dbac043d6a19c35420e77de1cb15886ad8a Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 23 Jun 2021 10:45:08 +0900 Subject: [PATCH 03/59] Add constellation calculation of GLONASS --- tool/navigation/GLONASS.h | 243 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 243 insertions(+) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index ba1a52829..2439fb4d6 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -264,6 +264,245 @@ static s ## bits ## _t name(const InputT *buf){ \ bool P4; // flag for ephemeris; 1 - uploaded by the control segment bool l_n; // health flag; 0 - healthy, 1 - malfunction + struct constellation_t { // TODO make it to be a subclass of System_XYZ + float_t position[3]; + float_t velocity[3]; + float_t pos_abs2() const { + float_t res(0); + for(int i(0); i < 3; ++i){ + res += (position[i] * position[i]); + } + return res; + } + constellation_t operator+(const constellation_t &another) const { // for RK4 + constellation_t res(*this); + for(int i(0); i < 3; ++i){ + res.position[i] += another.position[i]; + res.velocity[i] += another.velocity[i]; + } + return res; + } + constellation_t operator*(const float_t &sf) const { // for RK4 + constellation_t res(*this); + for(int i(0); i < 3; ++i){ + res.position[i] *= sf; + res.velocity[i] *= sf; + } + return res; + } + static const float_t omega_E; + constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ + // @see Appendix.A PZ-90.02 to O-X0Y0Z0 + float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); + float_t + x0(position[0] * crad - position[1] * srad), + y0(position[0] * srad + position[1] * crad); + constellation_t res = { + {x0, y0, position[2]}, + { + velocity[0] * crad - velocity[1] * srad - omega_E * y0, + velocity[0] * srad + velocity[1] * crad + omega_E * x0, + velocity[2] + } + }; + return res; + } + constellation_t rel_corrdinate(const float_t &sidereal_time_in_rad){ + // @see Appendix.A O-X0Y0Z0 to PZ-90.02 + float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); + float_t + x( position[0] * crad + position[1] * srad), + y(-position[0] * srad + position[1] * crad); + constellation_t res = { + {x, y, position[2]}, + { + velocity[0] * crad + velocity[1] * srad + omega_E * y, + -velocity[0] * srad + velocity[1] * crad - omega_E * x, + velocity[2] + } + }; + return res; + } + }; + + struct eccentric_anomaly_t { + float_t E_k; + float_t snu_k, cnu_k; + eccentric_anomaly_t(const float_t &g_k, const float_t &e_k){ + static const float_t delta_limit(1E-12); + for(int loop(0); loop < 10; loop++){ + float_t E_k2(g_k + e_k * std::sin(E_k)); + if(std::abs(E_k2 - E_k) < delta_limit){break;} + E_k = E_k2; + } + snu_k = std::sqrt(-e_k * e_k + 1) * std::sin(E_k) / (-e_k * std::cos(E_k) + 1); + cnu_k = (std::cos(E_k) - e_k) * std::sin(E_k) / (-e_k * std::cos(E_k) + 1); + } + }; + + struct lunar_solar_perturbations_t { + struct { + float_t xi, eta, zeta, r; + } m, s; + /** + * @param days Sum of days from the epoch at 00 hours Moscow Time (MT) on 1st January 1975 + * to the epoch at 00 hours MT of current date within which the instant t_e is. + * @param t_e reference time of ephemeris parameters (in Julian centuries of 36525 ephemeris days); + */ + lunar_solar_perturbations_t(const float_t &days, const float_t &t_e){ + #define dms2rad(deg, min, sec) \ + (M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg)) + static const float_t + a_m(3.84385243E8), // [m] + a_s(1.49598E11), //[m] + e_m(0.054900489), + e_s(0.016719), + i_m(dms2rad(5, 8, 43.4)), + epsilon(dms2rad(23, 26, 33)), + g_om(dms2rad(-63, 53, 43.41)), + g_1m(dms2rad(477198, 50, 56.79)), + Omega_om(dms2rad(259, 10, 59.79)), + Omega_1m(dms2rad(-1934, 8, 31.23)), + Gamma_o(dms2rad(-334, 19, 46.40)), + Gamma_1(dms2rad(4069, 2, 2.52)), + g_os(dms2rad(358, 28, 33.04)), + g_1s(dms2rad(0, 0, 129596579.10)); + + static const float_t + sf_ci_m(-std::cos(i_m) + 1), si_m(std::sin(i_m)), + cepsilon(std::cos(epsilon)), sepsilon(std::sin(epsilon)); + + float_t T((27392.375 + days + t_e / 86400) / 36525); + + { // Eq.(3) lunar (m) + float_t Omega_m(Omega_om + Omega_1m * T); + float_t sOmega_m(std::sin(Omega_m)), cOmega_m(std::sin(Omega_m)); + + float_t xi_ast(-cOmega_m * cOmega_m * sf_ci_m + 1); + float_t eta_ast(sOmega_m * si_m); + float_t zeta_ast(cOmega_m * si_m); + + float_t xi_11(sOmega_m * cOmega_m * sf_ci_m); + float_t xi_12(-sOmega_m * sOmega_m * sf_ci_m + 1); + + float_t eta_11(xi_ast * cepsilon - zeta_ast * sepsilon); + float_t eta_12(xi_11 * cepsilon + eta_ast * sepsilon); + + float_t zeta_11(xi_ast * sepsilon + zeta_ast * cepsilon); + float_t zeta_12(zeta_11 * sepsilon - eta_ast * cepsilon); + + float_t Gamma(Gamma_o + Gamma_1 * T); + float_t sGamma(std::sin(Gamma)), cGamma(std::cos(Gamma)); + + eccentric_anomaly_t E_m(g_om + g_1m * T, e_m); + + float_t + srad(E_m.snu_k * cGamma + E_m.cnu_k * sGamma), + crad(E_m.cnu_k * cGamma - E_m.snu_k * sGamma); + m.xi = srad * xi_11 + crad * xi_12; + m.eta = srad * eta_11 + crad * eta_12; + m.zeta = srad * zeta_11 + crad * zeta_12; + m.r = a_m * (-e_m * std::cos(E_m.E_k) + 1); + } + + { // Eq.(3) solar (s) + float_t omega_s(dms2rad(281, 13, 15.00 + (6189.03 * T))); + float_t co(std::cos(omega_s)), so(std::sin(omega_s)); + + eccentric_anomaly_t E_s(g_os + g_1s * T, e_s); + + float_t + srad(E_s.snu_k * co + E_s.cnu_k * so), // = sin(nu_s + omega_s) + crad(E_s.cnu_k * co - E_s.snu_k * so); // = cos(nu_s + omega_s) + + s.xi = crad; + s.eta = srad * cepsilon; + s.zeta = srad * sepsilon; + s.r = a_s * (-e_s * std::cos(E_s.E_k) + 1); + } + } + }; + + struct differential_t { + float_t J_x_a, J_y_a, J_z_a; + differential_t(const float_t &J_x, const float_t &J_y, const float_t &J_z) + : J_x_a(J_x), J_y_a(J_y), J_z_a(J_z) {} + differential_t(const Ephemeris &eph){ + float_t sidereal_time_in_rad(0); // TODO calculate from t_b + float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); + J_x_a = eph.xn_ddot * crad - eph.yn_ddot * srad; + J_y_a = eph.xn_ddot * srad + eph.yn_ddot * crad; + J_z_a = eph.zn_ddot; + } + constellation_t operator()(const float_t &t, const constellation_t &x) const { + // @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY + float_t r2(x.pos_abs2()), r(std::sqrt(r2)); + static const float_t + a_e(6378.136E3), mu(398600.44E9), C_20(-1082.63E-6); // TODO move to PZ-90 + float_t mu_bar(mu / r2), + x_a_bar(x.position[0] / r), y_a_bar(x.position[1] / r), + z_a_bar(x.position[2] / r), sf_z_a_bar(z_a_bar * z_a_bar * -5 + 1), + rho2(a_e * a_e / r2); + constellation_t res = { + x.velocity, + { // Eq.(1) + ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * x_a_bar + J_x_a, + ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * y_a_bar + J_y_a, + ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * z_a_bar + J_z_a, + } + }; + return res; + } + }; + + struct differential2_t { + lunar_solar_perturbations_t J_src; + constellation_t operator()(const float_t &t, const constellation_t &x) const { + // @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY + + float_t J_x_am, J_y_am, J_z_am; + { // Eq.(2) lunar (m) + float_t + mu_bar(4902.835E9 / std::pow(J_src.m.r, 2)), // mu_m in [m/s] + x_a_bar(x.position[0] / J_src.m.r), + y_a_bar(x.position[1] / J_src.m.r), + z_a_bar(x.position[2] / J_src.m.r), + delta_x(J_src.m.xi - x_a_bar), + delta_y(J_src.m.eta - y_a_bar), + delta_z(J_src.m.zeta - z_a_bar), + Delta3(std::pow( + std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5)); + J_x_am = mu_bar * (delta_x / Delta3 - J_src.m.xi); + J_y_am = mu_bar * (delta_y / Delta3 - J_src.m.eta); + J_z_am = mu_bar * (delta_z / Delta3 - J_src.m.zeta); + } + + float_t J_x_as, J_y_as, J_z_as; + { // Eq.(2) solar (s) + float_t + mu_bar(0.1325263E21 / std::pow(J_src.s.r, 2)), // mu_s in [m/s] + x_a_bar(x.position[0] / J_src.s.r), + y_a_bar(x.position[1] / J_src.s.r), + z_a_bar(x.position[2] / J_src.s.r), + delta_x(J_src.s.xi - x_a_bar), + delta_y(J_src.s.eta - y_a_bar), + delta_z(J_src.s.zeta - z_a_bar), + Delta3(std::pow( + std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5)); + J_x_as = mu_bar * (delta_x / Delta3 - J_src.s.xi); + J_y_as = mu_bar * (delta_y / Delta3 - J_src.s.eta); + J_z_as = mu_bar * (delta_z / Delta3 - J_src.s.zeta); + } + return differential_t(J_x_am + J_x_as, J_y_am + J_y_as, J_z_am + J_z_as)(t, x); + } + }; + + /*constellation_t constellation( + const float_t &t, const float_t &pseudo_range = 0, + const bool &with_velocity = true) const { + + }*/ + u8_t F_T_index() const; struct raw_t { @@ -478,6 +717,10 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} }; }; +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::omega_E + = 0.7292115E-4; // [s^-1] + #define POWER_2(n) \ (((n) >= 0) \ ? (float_t)(1 << (n >= 0 ? n : 0)) \ From d43bae7d716cf3676386f270a1dc714c5a3dc9fd Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 28 Jun 2021 16:19:45 +0900 Subject: [PATCH 04/59] Add interconnection between GPS and GLONASS times Constellation calculation is also included. --- tool/INS_GPS/GNSS_Data.h | 5 +- tool/navigation/GLONASS.h | 228 ++++++++++++++++++++++++++++++++++---- 2 files changed, 210 insertions(+), 23 deletions(-) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index c3beb8a14..61c37d49a 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -73,8 +73,8 @@ struct GNSS_Data { typedef typename gps_t::Ionospheric_UTC_Parameters gps_iono_utc_t; typedef GLONASS_SpaceNode glonass_t; - typedef typename glonass_t::Satellite::Ephemeris glonass_ephemeris_t; - struct glonass_ephemeris_raw_t : public glonass_ephemeris_t::raw_t, glonass_t::TimeProperties::raw_t { + typedef typename glonass_t::Satellite::Ephemeris_with_GPS_Time glonass_ephemeris_t; + struct glonass_ephemeris_raw_t : public glonass_ephemeris_t::raw_t { unsigned int super_frame; unsigned int has_string; glonass_ephemeris_raw_t() : has_string(0) {} @@ -198,7 +198,6 @@ struct GNSS_Data { } if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame glonass_ephemeris_t eph_converted(eph); - typename glonass_t::TimeProperties t_prop(eph); // TODO register ephemeris eph.has_string = 0; } diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 2439fb4d6..377768b58 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -42,8 +42,10 @@ #include #include #include +#include #include "GPS.h" +#include "algorithm/integral.h" template class GLONASS_SpaceNode { @@ -156,11 +158,65 @@ static s ## bits ## _t name(const InputT *buf){ \ }; struct TimeProperties { - float_t tau_c; // [s] - float_t tau_GPS; // [s] + float_t tau_c; ///< GLONASS time scale correction to UTC(SU) time [s] + float_t tau_GPS; ///< fractional part of time difference from GLONASS time to GPS time [s], integer part should be derived from another source struct date_t { uint_t year; // 20XX uint_t day_of_year; // Jan. 1st = 0 + /** + * @return (std::tm) + */ + std::tm c_tm() const { + static const uint_t days_m[] = { + 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 + }; + std::tm res = {0}; + res.tm_year = year - 1900; + res.tm_yday = res.tm_mday = day_of_year; // tm_yday ranges [0, 365] + do{ + if(res.tm_mday < days_m[0]){break;} // tm_mon ranges [0, 11], Check January + res.tm_mday -= days_m[0]; + ++res.tm_mon; + if((year % 400 == 0) || ((year % 4 == 0) && (year % 100 != 0))){ // is leap year? + if((res.tm_mday - 1) < days_m[1]){break;} // Check February + ++res.tm_mday; + } + for(; res.tm_mon < sizeof(days_m) / sizeof(days_m[0]); ++res.tm_mon){ + if(res.tm_mday < days_m[res.tm_mon]){break;} + res.tm_mday -= days_m[res.tm_mon]; + } + }while(false); + ++res.tm_mday; // tm_mday ranges [1, 31] + { + /* day of week according to (modified) Sakamoto's methods + * @see https://en.wikipedia.org/wiki/Determination_of_the_day_of_the_week#Sakamoto's_methods + */ + int_t y(year - 1); // year/1/1 + res.tm_wday = (y + y/4 - y/100 + y/400 + day_of_year + 1) % 7; + } + return res; + } + static float_t julian_day(const std::tm &date){ + // (5.48) of "Preliminary Orbit Determination" by Curtis, Howard D + // @see original Boulet(1991) + int y(date.tm_year + 1900), m(date.tm_mon + 1); + return 1721013.5 // expect all argument of (int) to be >= 0 + + 367 * y + - (int)(7 * (y + (int)((m + 9) / 12)) / 4) + + (int)(275 * m / 9) + + date.tm_mday; + } + static float_t Greenwich_sidereal_time_deg( + const std::tm &date, const float_t &ut_hr = 0){ + // @see "Preliminary Orbit Determination" by Curtis, Howard D + float_t T0((julian_day(date) - 2451545) / 36525); // (5.49) + float_t theta_G0_deg( // (5.50) + 100.4606184 + + 36000.77004 * T0 + + 0.000387933 * std::pow(T0, 2) + - 2.583E-8 * std::pow(T0, 3)); + return theta_G0_deg + 360.98564724 * ut_hr / 24; // (5.51) + } } date; bool l_n; @@ -238,16 +294,33 @@ static s ## bits ## _t name(const InputT *buf){ \ return *this; } }; + + bool is_equivalent(const TimeProperties &t) const { + do{ +#define CHECK(TARGET) if(TARGET != t.TARGET){break;} + CHECK(l_n); + CHECK(date.year); + CHECK(date.day_of_year); +#undef CHECK +#define CHECK(TARGET) \ +if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} + CHECK(tau_c); + CHECK(tau_GPS); +#undef CHECK + return true; + }while(false); + return false; + } }; class SatelliteProperties { public: struct Ephemeris { - uint_t t_k; // time base [s] - uint_t t_b; // time interval to UTC(SU) + 3hr [s] - uint_t M; // satellite type; 0 - GLONASS, 1 - GLONASS-M - float_t gamma_n; - float_t tau_n; // [s] + uint_t t_k; ///< time referenced to the beginning of the frame from current day [s] + uint_t t_b; ///< base time of ephemeris parameters in UTC(SU) + 3hr [s] + uint_t M; ///< satellite type; 0 - GLONASS, 1 - GLONASS-M + float_t gamma_n; ///< gamma_n(t_c = t_b) = d/dt (t_c - t_n) [dimensionless] + float_t tau_n; ///< tau_n(t_c = t_b) = t_c (GLONASS time) - t_n(n-th satellite time) [s] float_t xn, yn, zn; // [m] float_t xn_dot, yn_dot, zn_dot; // [m/s] float_t xn_ddot, yn_ddot, zn_ddot; // [m/s^2] @@ -290,6 +363,8 @@ static s ## bits ## _t name(const InputT *buf){ \ } return res; } + // TODO constant definitions should be moved to PZ-90.02 + static const float_t light_speed; static const float_t omega_E; constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ // @see Appendix.A PZ-90.02 to O-X0Y0Z0 @@ -350,8 +425,8 @@ static s ## bits ## _t name(const InputT *buf){ \ * @param t_e reference time of ephemeris parameters (in Julian centuries of 36525 ephemeris days); */ lunar_solar_perturbations_t(const float_t &days, const float_t &t_e){ - #define dms2rad(deg, min, sec) \ - (M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg)) +#define dms2rad(deg, min, sec) \ +(M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg)) static const float_t a_m(3.84385243E8), // [m] a_s(1.49598E11), //[m] @@ -420,15 +495,19 @@ static s ## bits ## _t name(const InputT *buf){ \ s.zeta = srad * sepsilon; s.r = a_s * (-e_s * std::cos(E_s.E_k) + 1); } +#undef dms2rad } }; struct differential_t { float_t J_x_a, J_y_a, J_z_a; + differential_t() : J_x_a(0), J_y_a(0), J_z_a(0) {} differential_t(const float_t &J_x, const float_t &J_y, const float_t &J_z) : J_x_a(J_x), J_y_a(J_y), J_z_a(J_z) {} - differential_t(const Ephemeris &eph){ - float_t sidereal_time_in_rad(0); // TODO calculate from t_b + /** + * @param sidereal_time_in_rad TODO calculate from t_b and UTC + */ + differential_t(const Ephemeris &eph, const float_t &sidereal_time_in_rad){ float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); J_x_a = eph.xn_ddot * crad - eph.yn_ddot * srad; J_y_a = eph.xn_ddot * srad + eph.yn_ddot * crad; @@ -497,12 +576,6 @@ static s ## bits ## _t name(const InputT *buf){ \ } }; - /*constellation_t constellation( - const float_t &t, const float_t &pseudo_range = 0, - const bool &with_velocity = true) const { - - }*/ - u8_t F_T_index() const; struct raw_t { @@ -681,7 +754,7 @@ static s ## bits ## _t name(const InputT *buf){ \ bool is_equivalent(const Ephemeris &eph) const { do{ #define CHECK(TARGET) if(TARGET != eph.TARGET){break;} - CHECK(t_k); + //CHECK(t_k); // t_k is just a reference time CHECK(t_b); CHECK(M); CHECK(B_n); @@ -710,6 +783,117 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return false; } }; + + /** + * + * Relationship of time systems + * 1) t_UTC = t_GL + tau_c - 3hr (polarity of tau_c is not defined in ICD!) + * 2) t_GPS = t_GL + delta_T + tau_GPS (defined in ICD) + * 3) t_n(t_b) = t_GL(t_b) - tau_n (@(t_UTC + 3hr) = t_b, defined in ICD) + * 4) t_n = t_GL - tau_n + gamma_n * (t_GL - t_b) (not explicitly defined in ICD, but in RINEX spec.) + */ + struct Ephemeris_with_Time : public Ephemeris, TimeProperties { + std::tm date_tm; + typedef typename Ephemeris::constellation_t constellation_t; + constellation_t xa_t_b; + float_t sidereal_t_b_rad; + typename Ephemeris::differntial_t eq_of_motion; + + struct raw_t : public Ephemeris::raw_t, TimeProperties::raw_t { + operator Ephemeris_with_Time() const { + Ephemeris_with_Time res; + (Ephemeris &)(res) = (Ephemeris)(*this); + (TimeProperties &)(res) = (TimeProperties)(*this); + + res.date_tm = res.date.c_tm(); + res.sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg( + res.date_tm, (float_t)res.t_b / (24 * 60 * 60) - 3) / 180 * M_PI; + constellation_t x_t_b = { + {res.xn, res.yn, res.zn}, + {res.xn_dot, res.yn_dot, res.zn_dot}, + }; + res.xa_t_b = x_t_b.abs_corrdinate(res.sidereal_t_b_rad); + res.eq_of_motion = typename Ephemeris::differntial_t(res, res.sidereal_t_b_rad); + return res; + } + raw_t &operator=(const Ephemeris_with_Time &eph){ + (typename Ephemeris::raw_t &)(*this) = eph; + (typename TimeProperties::raw_t &)(*this) = eph; + return *this; + } + }; + bool is_equivalent(const Ephemeris_with_Time &eph) const { + return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph); + } + float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const { + delta_t -= pseudo_range / constellation_t::light_speed; + return -Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; + } + /** + * @param t_arrival_glonass signal arrival time in GLONASS time scale (t_GL = t_UTC + 3 hr + tau_c). + */ + float_t clock_error( + const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const { + return calculate_clock_error( + t_arrival_glonass + TimeProperties::tau_c - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale + } + constellation_t calculate_constellation( + float_t delta_t, const float_t &pseudo_range = 0) const { + delta_t -= pseudo_range / constellation_t::light_speed; + + constellation_t res(xa_t_b); + { // time integration from t_b to t_arrival + float_t t_step_max(delta_t >= 0 ? 60 : -60); + int i(std::floor(delta_t / t_step_max)); + float_t t_step_remain(delta_t - t_step_max * i); + float_t delta_t_itg(0); // accumulative time of integration + for(; i > 0; --i, delta_t_itg += t_step_max){ + res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_max); + } + res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_remain); + } + + static const float_t omega_E(0.7292115E-4); // Earth's rotation rate, TODO move to PZ-90.02 + return res.rel_corrdinate(sidereal_t_b_rad + (omega_E * delta_t)); // transform from abs to PZ-90.02 + } + /** + * @param t_arrival_glonass signal arrival time in GLONASS time scale (t_GL = t_UTC + 3 hr + tau_c). + */ + constellation_t constellation( + const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const { + return calculate_constellation( + t_arrival_glonass + TimeProperties::tau_c - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale + } + }; + + struct Ephemeris_with_GPS_Time : public Ephemeris_with_Time { + GPS_Time t_b_gps; + /** + * + * @param deltaT integer part of difference of GPS and GLONASS time scales. + * This is often identical to the leap seconds because GLONASS time base is UTC + */ + Ephemeris_with_GPS_Time( + const typename Ephemeris_with_Time::raw_t &raw, + const int_t &deltaT = 0) + : Ephemeris_with_Time(raw), + t_b_gps(GPS_Time(Ephemeris_with_Time::date_tm) + + (Ephemeris_with_Time::t_b - 3 * 60 * 60) // in UTC scale + - Ephemeris_with_Time::tau_c // in (glonass - 3hr) scale + + Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer) + + deltaT) { + } + using Ephemeris_with_Time::clock_error; + float_t clock_error( + const GPS_Time &t_arrival, const float_t &pseudo_range = 0) const { + return Ephemeris_with_Time::calculate_clock_error(t_arrival - t_b_gps, pseudo_range); + } + using Ephemeris_with_Time::constellation; + typename Ephemeris_with_Time::constellation_t constellation( + const GPS_Time &t_arrival, const float_t &pseudo_range = 0) const { + return calculate_constellation(t_arrival - t_b_gps, pseudo_range); + } + }; }; struct Satellite : public SatelliteProperties { @@ -717,6 +901,10 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} }; }; +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::light_speed + = 299792458; // [m/s] + template const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::omega_E = 0.7292115E-4; // [s^-1] @@ -740,8 +928,8 @@ const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::Sat POWER_2(-30) * 1E3, // x_ddot 15 * 60, // t_b POWER_2(-40), // gamma_n - POWER_2(-30), // tau - POWER_2(-30), // delta_tau + POWER_2(-30), // tau_n + POWER_2(-30), // delta_tau_n }; #undef POWER_2 From db10be95a1b170fb6956a81c86441b6e20d556b7 Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 5 Jul 2021 12:11:45 +0900 Subject: [PATCH 05/59] Add correction of leap seconds to GPS time in GLONASS ephemeris --- tool/INS_GPS/GNSS_Data.h | 3 +++ tool/navigation/GLONASS.h | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index 61c37d49a..18204f260 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -198,6 +198,9 @@ struct GNSS_Data { } if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame glonass_ephemeris_t eph_converted(eph); + eph_converted.t_b_gps += gps->is_valid_utc() // leap second correction + ? gps->iono_utc().delta_t_LS + : gps_time_t::guess_leap_seconds(eph_converted.t_b_gps); // TODO register ephemeris eph.has_string = 0; } diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 377768b58..eaed6eb02 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -797,7 +797,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} typedef typename Ephemeris::constellation_t constellation_t; constellation_t xa_t_b; float_t sidereal_t_b_rad; - typename Ephemeris::differntial_t eq_of_motion; + typename Ephemeris::differential_t eq_of_motion; struct raw_t : public Ephemeris::raw_t, TimeProperties::raw_t { operator Ephemeris_with_Time() const { @@ -813,7 +813,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} {res.xn_dot, res.yn_dot, res.zn_dot}, }; res.xa_t_b = x_t_b.abs_corrdinate(res.sidereal_t_b_rad); - res.eq_of_motion = typename Ephemeris::differntial_t(res, res.sidereal_t_b_rad); + res.eq_of_motion = typename Ephemeris::differential_t(res, res.sidereal_t_b_rad); return res; } raw_t &operator=(const Ephemeris_with_Time &eph){ From 54dca6e857ca3c9fae3be1929e181ac1129fe718 Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 3 Jan 2022 07:52:47 +0900 Subject: [PATCH 06/59] Add GLONASS frequency channel handling --- tool/INS_GPS.cpp | 1 + tool/INS_GPS/GNSS_Data.h | 2 ++ tool/navigation/GLONASS.h | 38 ++++++++++++++++++++++++++++++++++---- 3 files changed, 37 insertions(+), 4 deletions(-) diff --git a/tool/INS_GPS.cpp b/tool/INS_GPS.cpp index f69786ed1..fe78b4053 100644 --- a/tool/INS_GPS.cpp +++ b/tool/INS_GPS.cpp @@ -1968,6 +1968,7 @@ class StreamProcessor if(bytes > sizeof(packet.subframe.buffer)){return;} observer.inspect(packet.subframe.buffer, (packet.subframe.bytes = bytes), 6 + 8); if((G_Observer_t::u8_t)observer[6 + 2] != 0){return;} // TODO reserved1, sigID? (0:L1C/A, 4:L2CM?) + packet.subframe.glonass_freq_ch = -7 + (int)(G_Observer_t::u8_t)observer[6 + 3]; check_subframeX( (G_Observer_t::u8_t)observer[6 + 0], (G_Observer_t::u8_t)observer[6 + 1], diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index 18204f260..a843a16b4 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -52,6 +52,7 @@ struct GNSS_Data { unsigned int sv_number; unsigned int bytes; typename observer_t::v8_t buffer[40]; + int glonass_freq_ch; } subframe; typedef GPS_Time gps_time_t; @@ -198,6 +199,7 @@ struct GNSS_Data { } if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame glonass_ephemeris_t eph_converted(eph); + eph_converted.freq_ch = data.subframe.glonass_freq_ch; // frequncy channel eph_converted.t_b_gps += gps->is_valid_utc() // leap second correction ? gps->iono_utc().delta_t_LS : gps_time_t::guess_leap_seconds(eph_converted.t_b_gps); diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index eaed6eb02..a512ec10f 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -52,6 +52,12 @@ class GLONASS_SpaceNode { public: typedef FloatT float_t; + static const float_t light_speed; + static const float_t L1_frequency; + static const float_t L1_frequency_gap; + static const float_t L2_frequency; + static const float_t L2_frequency_gap; + public: typedef GLONASS_SpaceNode self_t; @@ -316,6 +322,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} class SatelliteProperties { public: struct Ephemeris { + int_t freq_ch; uint_t t_k; ///< time referenced to the beginning of the frame from current day [s] uint_t t_b; ///< base time of ephemeris parameters in UTC(SU) + 3hr [s] uint_t M; ///< satellite type; 0 - GLONASS, 1 - GLONASS-M @@ -337,6 +344,14 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} bool P4; // flag for ephemeris; 1 - uploaded by the control segment bool l_n; // health flag; 0 - healthy, 1 - malfunction + float_t frequncy_L1() const { + return L1_frequency + L1_frequency_gap * freq_ch; + } + + float_t frequncy_L2() const { + return L2_frequency + L2_frequency_gap * freq_ch; + } + struct constellation_t { // TODO make it to be a subclass of System_XYZ float_t position[3]; float_t velocity[3]; @@ -364,7 +379,6 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return res; } // TODO constant definitions should be moved to PZ-90.02 - static const float_t light_speed; static const float_t omega_E; constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ // @see Appendix.A PZ-90.02 to O-X0Y0Z0 @@ -826,7 +840,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph); } float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const { - delta_t -= pseudo_range / constellation_t::light_speed; + delta_t -= pseudo_range / light_speed; return -Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; } /** @@ -839,7 +853,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } constellation_t calculate_constellation( float_t delta_t, const float_t &pseudo_range = 0) const { - delta_t -= pseudo_range / constellation_t::light_speed; + delta_t -= pseudo_range / light_speed; constellation_t res(xa_t_b); { // time integration from t_b to t_arrival @@ -902,9 +916,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} }; template -const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::light_speed +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::light_speed = 299792458; // [m/s] +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_frequency + = 1602E6; // [Hz] + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_frequency_gap + = 562.5E3; // [Hz] + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L2_frequency + = 1246E6; // [Hz] + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L2_frequency_gap + = 437.5E3; // [Hz] + template const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::omega_E = 0.7292115E-4; // [s^-1] From 07cf8846697fc564cec132f2c861b9dbadfb65fe Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 5 Jan 2022 00:29:57 +0900 Subject: [PATCH 07/59] Refactor GLONASS time related code --- tool/INS_GPS/GNSS_Data.h | 1 + tool/navigation/GLONASS.h | 103 +++++++++++++++++++++++++++----------- 2 files changed, 75 insertions(+), 29 deletions(-) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index a843a16b4..4ac6bb7f6 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -198,6 +198,7 @@ struct GNSS_Data { } } if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame + // Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time glonass_ephemeris_t eph_converted(eph); eph_converted.freq_ch = data.subframe.glonass_freq_ch; // frequncy channel eph_converted.t_b_gps += gps->is_valid_utc() // leap second correction diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index a512ec10f..fd9676ac7 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -167,15 +167,13 @@ static s ## bits ## _t name(const InputT *buf){ \ float_t tau_c; ///< GLONASS time scale correction to UTC(SU) time [s] float_t tau_GPS; ///< fractional part of time difference from GLONASS time to GPS time [s], integer part should be derived from another source struct date_t { - uint_t year; // 20XX - uint_t day_of_year; // Jan. 1st = 0 + int_t year; // 20XX + int_t day_of_year; // Jan. 1st = 0 + static const int_t days_m[12]; /** * @return (std::tm) */ std::tm c_tm() const { - static const uint_t days_m[] = { - 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 - }; std::tm res = {0}; res.tm_year = year - 1900; res.tm_yday = res.tm_mday = day_of_year; // tm_yday ranges [0, 365] @@ -187,7 +185,7 @@ static s ## bits ## _t name(const InputT *buf){ \ if((res.tm_mday - 1) < days_m[1]){break;} // Check February ++res.tm_mday; } - for(; res.tm_mon < sizeof(days_m) / sizeof(days_m[0]); ++res.tm_mon){ + for(; res.tm_mon < (int)(sizeof(days_m) / sizeof(days_m[0])); ++res.tm_mon){ if(res.tm_mday < days_m[res.tm_mon]){break;} res.tm_mday -= days_m[res.tm_mon]; } @@ -202,6 +200,23 @@ static s ## bits ## _t name(const InputT *buf){ \ } return res; } + static date_t from_c_tm(const std::tm &date){ + static const struct days_t { + int_t sum_m[sizeof(days_m) / sizeof(days_m[0])]; + days_t(){ + sum_m[0] = 0; + for(unsigned int i(1); i < sizeof(days_m) / sizeof(days_m[0]); ++i){ + sum_m[i] = sum_m[i - 1] + days_m[i - 1]; + } + } + } days; + date_t res = {date.tm_year + 1900, days.sum_m[date.tm_mon] + date.tm_mday - 1}; + if((date.tm_mon >= 2) + && ((res.year % 400 == 0) || ((res.year % 4 == 0) && (res.year % 100 != 0)))){ + ++res.day_of_year; // leap year + } + return res; + } static float_t julian_day(const std::tm &date){ // (5.48) of "Preliminary Orbit Determination" by Curtis, Howard D // @see original Boulet(1991) @@ -224,7 +239,7 @@ static s ## bits ## _t name(const InputT *buf){ \ return theta_G0_deg + 360.98564724 * ut_hr / 24; // (5.51) } } date; - bool l_n; + bool l_n; // health flag; 0 - healthy, 1 - malfunction struct raw_t { s32_t tau_c; @@ -807,28 +822,45 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} * 4) t_n = t_GL - tau_n + gamma_n * (t_GL - t_b) (not explicitly defined in ICD, but in RINEX spec.) */ struct Ephemeris_with_Time : public Ephemeris, TimeProperties { - std::tm date_tm; typedef typename Ephemeris::constellation_t constellation_t; constellation_t xa_t_b; float_t sidereal_t_b_rad; typename Ephemeris::differential_t eq_of_motion; + void calculate_additional() { + sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg( + TimeProperties::date.c_tm(), + (float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI; + constellation_t x_t_b = { + {this->xn, this->yn, this->zn}, + {this->xn_dot, this->yn_dot, this->zn_dot}, + }; + xa_t_b = x_t_b.abs_corrdinate(sidereal_t_b_rad); + eq_of_motion = typename Ephemeris::differential_t((*this), sidereal_t_b_rad); + } + Ephemeris_with_Time(const Ephemeris &eph, const TimeProperties &t_prop) + : Ephemeris(eph), TimeProperties(t_prop) { + calculate_additional(); + } + Ephemeris_with_Time(const Ephemeris &eph, const std::tm &t_utc) + : Ephemeris(eph) { + this->tau_c = this->tau_GPS = 0; + std::tm t_mt(t_utc); // calculate Moscow time + t_mt.tm_hour += 3; + std::mktime(&t_mt); // renormalization + this->date = TimeProperties::date_t::from_c_tm(t_mt); + this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec; + calculate_additional(); + } + std::tm c_tm_utc() const { + std::tm t(TimeProperties::date.c_tm()); // set date on Moscow time + (t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC + std::mktime(&t); // renormalization + return t; + } struct raw_t : public Ephemeris::raw_t, TimeProperties::raw_t { operator Ephemeris_with_Time() const { - Ephemeris_with_Time res; - (Ephemeris &)(res) = (Ephemeris)(*this); - (TimeProperties &)(res) = (TimeProperties)(*this); - - res.date_tm = res.date.c_tm(); - res.sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg( - res.date_tm, (float_t)res.t_b / (24 * 60 * 60) - 3) / 180 * M_PI; - constellation_t x_t_b = { - {res.xn, res.yn, res.zn}, - {res.xn_dot, res.yn_dot, res.zn_dot}, - }; - res.xa_t_b = x_t_b.abs_corrdinate(res.sidereal_t_b_rad); - res.eq_of_motion = typename Ephemeris::differential_t(res, res.sidereal_t_b_rad); - return res; + return Ephemeris_with_Time((Ephemeris)(*this), (TimeProperties)(*this)); } raw_t &operator=(const Ephemeris_with_Time &eph){ (typename Ephemeris::raw_t &)(*this) = eph; @@ -882,20 +914,28 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} struct Ephemeris_with_GPS_Time : public Ephemeris_with_Time { GPS_Time t_b_gps; + Ephemeris_with_GPS_Time() + : Ephemeris_with_Time(Ephemeris(), GPS_Time(0, 0).c_tm()), + t_b_gps(0, 0) {} /** * * @param deltaT integer part of difference of GPS and GLONASS time scales. * This is often identical to the leap seconds because GLONASS time base is UTC */ Ephemeris_with_GPS_Time( - const typename Ephemeris_with_Time::raw_t &raw, + const typename Ephemeris_with_Time &eph, const int_t &deltaT = 0) - : Ephemeris_with_Time(raw), - t_b_gps(GPS_Time(Ephemeris_with_Time::date_tm) - + (Ephemeris_with_Time::t_b - 3 * 60 * 60) // in UTC scale - - Ephemeris_with_Time::tau_c // in (glonass - 3hr) scale - + Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer) - + deltaT) { + : Ephemeris_with_Time(eph), + t_b_gps((GPS_Time(eph.c_tm_utc()) // in UTC scale + + (-Ephemeris_with_Time::tau_c // in Moscow Time scale + + Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer) + + deltaT))) { + } + GPS_Time base_time() const { + return t_b_gps; + } + bool is_valid(const GPS_Time &t) const { + return std::abs(t_b_gps.interval(t)) <= 60 * 60; // 1 hour } using Ephemeris_with_Time::clock_error; float_t clock_error( @@ -939,6 +979,11 @@ template const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::SatelliteProperties::Ephemeris::constellation_t::omega_E = 0.7292115E-4; // [s^-1] +template +const typename GLONASS_SpaceNode::int_t GLONASS_SpaceNode::TimeProperties::date_t::days_m[] = { + 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 +}; + #define POWER_2(n) \ (((n) >= 0) \ ? (float_t)(1 << (n >= 0 ? n : 0)) \ From 306c1bc314651a633f5074eafd6da020342d4a79 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 6 Jan 2022 11:28:49 +0900 Subject: [PATCH 08/59] Add GLONASS space nodes --- tool/navigation/GLONASS.h | 95 +++++++++++++++++++++++++++++++++++++-- 1 file changed, 92 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index fd9676ac7..9b23aae23 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -922,9 +922,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} * @param deltaT integer part of difference of GPS and GLONASS time scales. * This is often identical to the leap seconds because GLONASS time base is UTC */ - Ephemeris_with_GPS_Time( - const typename Ephemeris_with_Time &eph, - const int_t &deltaT = 0) + Ephemeris_with_GPS_Time(const Ephemeris_with_Time &eph, const int_t &deltaT = 0) : Ephemeris_with_Time(eph), t_b_gps((GPS_Time(eph.c_tm_utc()) // in UTC scale + (-Ephemeris_with_Time::tau_c // in Moscow Time scale @@ -951,8 +949,99 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} }; struct Satellite : public SatelliteProperties { + public: + typedef typename SatelliteProperties::Ephemeris_with_GPS_Time eph_t; + typedef typename GPS_SpaceNode::template PropertyHistory eph_list_t; + protected: + eph_list_t eph_history; + public: + Satellite() : eph_history() { + // TODO setup first ephemeris as invalid one + // eph_t &eph_current(const_cast(eph_history.current())); + } + + template + void each_ephemeris( + Functor &functor, + const typename eph_list_t::each_mode_t &mode = eph_list_t::EACH_ALL) const { + eph_history.each(functor, mode); + } + + void register_ephemeris(const eph_t &eph, const int &priority_delta = 1){ + eph_history.add(eph, priority_delta); + } + + void merge(const Satellite &another, const bool &keep_original = true){ + eph_history.merge(another.eph_history, keep_original); + } + + const eph_t &ephemeris() const { + return eph_history.current(); + } + + /** + * Select appropriate ephemeris within registered ones. + * + * @param target_time time at measurement + * @return if true, appropriate ephemeris is selected, otherwise, not selected. + */ + bool select_ephemeris(const GPS_Time &target_time){ + return eph_history.select(target_time, &eph_t::is_valid); + } + + float_t clock_error(const GPS_Time &t, const float_t &pseudo_range = 0) const{ + return ephemeris().clock_error(t, pseudo_range); + } + + typename GPS_SpaceNode::SatelliteProperties::constellation_t constellation( + const GPS_Time &t, const float_t &pseudo_range = 0) const { + typename eph_t::constellation_t constellation_PZ9002( + ephemeris().constellation(t, pseudo_range)); + // @see https://www.gsi.go.jp/common/000070971.pdf + // @see (originally) Federal Air Navigation Authority (FANA), Aeronautical Information Circular + // of the Russian Federation, 12 February 2009, Russia. + float_t (&pos)[3](constellation_PZ9002.position); + float_t (&vel)[3](constellation_PZ9002.velocity); + typename GPS_SpaceNode::SatelliteProperties::constellation_t res = { + typename GPS_SpaceNode::xyz_t(pos[0] - 0.36, pos[1] + 0.08, pos[2] + 0.18), + typename GPS_SpaceNode::xyz_t(vel[0], vel[1], vel[2]), + }; + return res; + } + + typename GPS_SpaceNode::xyz_t position(const GPS_Time &t, const float_t &pseudo_range = 0) const { + return constellation(t, pseudo_range).position; + } + typename GPS_SpaceNode::xyz_t velocity(const GPS_Time &t, const float_t &pseudo_range = 0) const { + return constellation(t, pseudo_range).velocity; + } }; + public: + typedef std::map satellites_t; + protected: + satellites_t _satellites; + public: + GLONASS_SpaceNode() + : _satellites() {} + ~GLONASS_SpaceNode(){ + _satellites.clear(); + } + const satellites_t &satellites() const { + return _satellites; + } + Satellite &satellite(const int &prn) { + return _satellites[prn]; + } + bool has_satellite(const int &prn) const { + return _satellites.find(prn) != _satellites.end(); + } + void update_all_ephemeris(const GPS_Time &target_time) { + for(typename satellites_t::iterator it(_satellites.begin()), it_end(_satellites.end()); + it != it_end; ++it){ + it->second.select_ephemeris(target_time); + } + } }; template From 0a15bb45faea1e826cd7a30e4c06f22ac5e6fd77 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 7 Jan 2022 00:05:16 +0900 Subject: [PATCH 09/59] Add redundant svid variable to GLONASS::Ephemeris --- tool/INS_GPS/GNSS_Data.h | 3 +++ tool/navigation/GLONASS.h | 5 +++++ 2 files changed, 8 insertions(+) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index 4ac6bb7f6..cf316fcca 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -86,6 +86,9 @@ struct GNSS_Data { for(unsigned int i(0); i < sizeof(gps_ephemeris) / sizeof(gps_ephemeris[0]); ++i){ gps_ephemeris[i].svid = i + 1; } + for(unsigned int i(0); i < sizeof(glonass_ephemeris) / sizeof(glonass_ephemeris[0]); ++i){ + glonass_ephemeris[i].svid = i + 1; + } } /** diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 9b23aae23..ff164cc6c 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -337,6 +337,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} class SatelliteProperties { public: struct Ephemeris { + uint_t svid; int_t freq_ch; uint_t t_k; ///< time referenced to the beginning of the frame from current day [s] uint_t t_b; ///< base time of ephemeris parameters in UTC(SU) + 3hr [s] @@ -608,6 +609,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} u8_t F_T_index() const; struct raw_t { + u8_t svid; + // String1 u8_t P1; u16_t t_k; @@ -704,6 +707,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} Ephemeris res; #define CONVERT(TARGET) \ {res.TARGET = sf[SF_ ## TARGET] * TARGET;} + res.svid = svid; res.t_k = (((t_k >> 7) & 0x1F) * 3600) // hour + (((t_k >> 1) & 0x3F) * 60) // min + ((t_k & 0x1) ? 30 : 0); // sec @@ -743,6 +747,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} // TODO: m? #define CONVERT(TARGET) \ {TARGET = (s32_t)((eph.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);} + svid = eph.svid; { // t_k std::div_t minutes(div(eph.t_k, 60)); std::div_t hrs(div(minutes.quot, 60)); From 279a91279f44e5120d092817f6f0612ea00dbaa0 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 7 Jan 2022 09:48:01 +0900 Subject: [PATCH 10/59] Add GLONASS test skeleton --- tool/NinjaScanLight_tools.sln | 8 ++ tool/test/test_GLONASS.cpp | 46 ++++++++++ tool/test/test_GLONASS.vcxproj | 152 +++++++++++++++++++++++++++++++++ 3 files changed, 206 insertions(+) create mode 100644 tool/test/test_GLONASS.cpp create mode 100644 tool/test/test_GLONASS.vcxproj diff --git a/tool/NinjaScanLight_tools.sln b/tool/NinjaScanLight_tools.sln index 7704749ab..8e03faef6 100644 --- a/tool/NinjaScanLight_tools.sln +++ b/tool/NinjaScanLight_tools.sln @@ -29,6 +29,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test_SP3", "test\test_SP3.v EndProject Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test_ANTEX", "test\test_ANTEX.vcxproj", "{3773A75D-B42D-414E-BA11-7596ACCA54C4}" EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "test_GLONASS", "test\test_GLONASS.vcxproj", "{6E52320A-5127-4EBF-B6C1-A6DA14D67987}" +EndProject Global GlobalSection(SolutionConfigurationPlatforms) = preSolution AppVeyor|Win32 = AppVeyor|Win32 @@ -114,6 +116,12 @@ Global {3773A75D-B42D-414E-BA11-7596ACCA54C4}.Debug|Win32.Build.0 = Debug|Win32 {3773A75D-B42D-414E-BA11-7596ACCA54C4}.Release|Win32.ActiveCfg = Release|Win32 {3773A75D-B42D-414E-BA11-7596ACCA54C4}.Release|Win32.Build.0 = Release|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.AppVeyor|Win32.ActiveCfg = AppVeyor|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.AppVeyor|Win32.Build.0 = AppVeyor|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.Debug|Win32.ActiveCfg = Debug|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.Debug|Win32.Build.0 = Debug|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.Release|Win32.ActiveCfg = Release|Win32 + {6E52320A-5127-4EBF-B6C1-A6DA14D67987}.Release|Win32.Build.0 = Release|Win32 EndGlobalSection GlobalSection(SolutionProperties) = preSolution HideSolutionNode = FALSE diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp new file mode 100644 index 000000000..846bd1d4f --- /dev/null +++ b/tool/test/test_GLONASS.cpp @@ -0,0 +1,46 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "navigation/GLONASS.h" + +#include +#include + +#include +#include + +#define BOOST_TEST_MAIN +#include + +using namespace std; +using boost::format; + +typedef GLONASS_SpaceNode space_node_t; + +BOOST_AUTO_TEST_SUITE(GLONASS) + +struct Fixture { + boost::random::mt19937 gen; + boost::random::uniform_int_distribution<> bin_dist; + + Fixture() + : gen(0), //static_cast(time(0)) + bin_dist(0, 1) + {} + ~Fixture(){} + + bool get_bool(){ + return bin_dist(gen) == 1; + } +}; + +BOOST_AUTO_TEST_CASE(ephemeris){ + +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/tool/test/test_GLONASS.vcxproj b/tool/test/test_GLONASS.vcxproj new file mode 100644 index 000000000..43a376f7a --- /dev/null +++ b/tool/test/test_GLONASS.vcxproj @@ -0,0 +1,152 @@ +ｻｿ + + + + AppVeyor + Win32 + + + Debug + Win32 + + + Release + Win32 + + + + {6E52320A-5127-4EBF-B6C1-A6DA14D67987} + log_CSV + Win32Proj + test_GLONASS + + + + Application + Unicode + true + + + Application + Unicode + true + + + Application + Unicode + + + + + + + + + + + + + + + + <_ProjectFileVersion>10.0.30319.1 + $(SolutionDir)build_VC\$(Configuration)\ + $(SolutionDir)build_VC\$(Configuration)\$(ProjectName)\ + true + $(SolutionDir)build_VC\$(Configuration)\ + $(SolutionDir)build_VC\$(Configuration)\ + $(SolutionDir)build_VC\$(Configuration)\$(ProjectName)\ + $(SolutionDir)build_VC\$(Configuration)\$(ProjectName)\ + false + false + + + + Disabled + $(ProjectDir)..;C:\Program Files\Microsoft Platform SDK\include;%(AdditionalIncludeDirectories) + WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions) + true + EnableFastChecks + MultiThreadedDebugDLL + NotUsing + Level3 + EditAndContinue + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + + + true + Console + MachineX86 + + + + + MaxSpeed + true + $(ProjectDir)..;C:\Program Files\Microsoft Platform SDK\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreadedDLL + true + NotUsing + Level3 + ProgramDatabase + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + + + true + Console + true + true + MachineX86 + + + + + MaxSpeed + true + $(ProjectDir)..;C:\Program Files\Microsoft Platform SDK\include;%(AdditionalIncludeDirectories) + WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions) + MultiThreadedDLL + true + NotUsing + Level3 + ProgramDatabase + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + $(IntDir)%(RelativeDir) + + + true + Console + true + true + MachineX86 + + + + + + + + + + + + + + + + + + 縺薙ｮ繝励Ο繧ｸ繧ｧ繧ｯ繝医ｯ縲√％縺ｮ繧ｳ繝ｳ繝斐Η繝ｼ繧ｿ繝ｼ荳翫↓縺ｪ縺 NuGet 繝代ャ繧ｱ繝ｼ繧ｸ繧貞盾辣ｧ縺励※縺縺ｾ縺吶ゅ◎繧後ｉ縺ｮ繝代ャ繧ｱ繝ｼ繧ｸ繧偵ム繧ｦ繝ｳ繝ｭ繝ｼ繝峨☆繧九↓縺ｯ縲ーNuGet 繝代ャ繧ｱ繝ｼ繧ｸ縺ｮ蠕ｩ蜈ゾ 繧剃ｽｿ逕ｨ縺励∪縺吶りｩｳ邏ｰ縺ｫ縺､縺縺ｦ縺ｯ縲”ttp://go.microsoft.com/fwlink/?LinkID=322105 繧貞盾辣ｧ縺励※縺上□縺輔＞縲りｦ九▽縺九ｉ縺ｪ縺繝輔ぃ繧､繝ｫ縺ｯ {0} 縺ｧ縺吶 + + + + + + + \ No newline at end of file From 3e786e47bcf1bb7acf3e69b8cf85afbae7f23384 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 8 Jan 2022 00:34:54 +0900 Subject: [PATCH 11/59] Fix date calculation and check with sidereal time test --- tool/navigation/GLONASS.h | 5 +++-- tool/test/test_GLONASS.cpp | 30 +++++++++++++++++++++++++++++- 2 files changed, 32 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index ff164cc6c..2b7f366f5 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -182,8 +182,9 @@ static s ## bits ## _t name(const InputT *buf){ \ res.tm_mday -= days_m[0]; ++res.tm_mon; if((year % 400 == 0) || ((year % 4 == 0) && (year % 100 != 0))){ // is leap year? - if((res.tm_mday - 1) < days_m[1]){break;} // Check February - ++res.tm_mday; + if(res.tm_mday < (days_m[1] + 1)){break;} // Check February in leap year + res.tm_mday -= (days_m[1] + 1); + ++res.tm_mon; } for(; res.tm_mon < (int)(sizeof(days_m) / sizeof(days_m[0])); ++res.tm_mon){ if(res.tm_mday < days_m[res.tm_mon]){break;} diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp index 846bd1d4f..0c099071e 100644 --- a/tool/test/test_GLONASS.cpp +++ b/tool/test/test_GLONASS.cpp @@ -1,5 +1,10 @@ +#if defined(_MSC_VER) +#define _USE_MATH_DEFINES +#endif + #include #include +#include #include #include #include @@ -39,8 +44,31 @@ struct Fixture { } }; -BOOST_AUTO_TEST_CASE(ephemeris){ +BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_K){ + space_node_t::TimeProperties::raw_t raw = {0}; + raw.N_4 = 5; + raw.NA = 251; + space_node_t::TimeProperties t(raw); + std::tm t_tm(t.date.c_tm()); + BOOST_REQUIRE_EQUAL(t_tm.tm_year, 2012 - 1900); + BOOST_REQUIRE_EQUAL(t_tm.tm_mon, 9 - 1); + BOOST_REQUIRE_EQUAL(t_tm.tm_mday, 7); + + { // additional tests to Appendix.K + space_node_t::TimeProperties::date_t date( + space_node_t::TimeProperties::date_t::from_c_tm(t_tm)); + BOOST_REQUIRE_EQUAL(t.date.year, date.year); + BOOST_REQUIRE_EQUAL(t.date.day_of_year, date.day_of_year); + } + + double jd0(space_node_t::TimeProperties::date_t::julian_day(t_tm)); + BOOST_REQUIRE_CLOSE(jd0, 2456177.5, 1E-8); + double gmst_deg(std::fmod( + space_node_t::TimeProperties::date_t::Greenwich_sidereal_time_deg(t_tm), + 360)); + BOOST_REQUIRE_CLOSE( + gmst_deg, std::fmod(29191.442830, M_PI * 2) / M_PI * 180, 1E-3); } BOOST_AUTO_TEST_SUITE_END() From ab6a02b52db4612b9817c552c7e2d01583efeea5 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 11 Jan 2022 00:06:17 +0900 Subject: [PATCH 12/59] Fix constellation calculation with examples in ICD --- tool/navigation/GLONASS.h | 28 +++++++++++++++++-------- tool/test/test_GLONASS.cpp | 43 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 62 insertions(+), 9 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 2b7f366f5..ee45cfd65 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -395,6 +395,9 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } return res; } + constellation_t operator/(const float_t &sf) const { // for RK4 + return operator*(((float_t)1)/sf); + } // TODO constant definitions should be moved to PZ-90.02 static const float_t omega_E; constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ @@ -548,19 +551,26 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} // @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY float_t r2(x.pos_abs2()), r(std::sqrt(r2)); static const float_t - a_e(6378.136E3), mu(398600.44E9), C_20(-1082.63E-6); // TODO move to PZ-90 + a_e(6378136), mu(398600.44E9), C_20(1082625.75E-9); // values are obtained from ver.5.1; TODO move to PZ-90 float_t mu_bar(mu / r2), - x_a_bar(x.position[0] / r), y_a_bar(x.position[1] / r), - z_a_bar(x.position[2] / r), sf_z_a_bar(z_a_bar * z_a_bar * -5 + 1), + x_a_bar(x.position[0] / r), y_a_bar(x.position[1] / r), z_a_bar(x.position[2] / r), + sf_z_a_bar(z_a_bar * z_a_bar * -5 + 1), rho2(a_e * a_e / r2); constellation_t res = { - x.velocity, - { // Eq.(1) - ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * x_a_bar + J_x_a, - ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * y_a_bar + J_y_a, - ((C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * z_a_bar + J_z_a, + {x.velocity[0], x.velocity[1], x.velocity[2]}, + { // @see ver.5.1 Eq.(1) + ((-C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * x_a_bar + J_x_a, + ((-C_20 * rho2 * sf_z_a_bar * 3 / 2) - 1) * mu_bar * y_a_bar + J_y_a, + ((-C_20 * rho2 * (sf_z_a_bar + 2) * 3 / 2) - 1) * mu_bar * z_a_bar + J_z_a, } }; +#if 0 + // If integration is performed in PZ-90, then these additional term is required, + // This is derived form simplified algorithm in CDMA ICD, (neither ver 5.1 nor ver.4 ICDs use it) + static const float_t omega_E(7.2921151467E-5), omega_E2(omega_E * omega_E); + res.velocity[0] += omega_E2 * x.position[0] + omega_E * 2 * x.velocity[1]; + res.velocity[1] += omega_E2 * x.position[1] - omega_E * 2 * x.velocity[0]; +#endif return res; } }; @@ -841,7 +851,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} {this->xn, this->yn, this->zn}, {this->xn_dot, this->yn_dot, this->zn_dot}, }; - xa_t_b = x_t_b.abs_corrdinate(sidereal_t_b_rad); + xa_t_b = x_t_b.abs_corrdinate(sidereal_t_b_rad); // PZ-90 => O-XYZ eq_of_motion = typename Ephemeris::differential_t((*this), sidereal_t_b_rad); } Ephemeris_with_Time(const Ephemeris &eph, const TimeProperties &t_prop) diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp index 0c099071e..c593c421d 100644 --- a/tool/test/test_GLONASS.cpp +++ b/tool/test/test_GLONASS.cpp @@ -71,4 +71,47 @@ BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_K){ gmst_deg, std::fmod(29191.442830, M_PI * 2) / M_PI * 180, 1E-3); } +BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_J){ + space_node_t::SatelliteProperties::Ephemeris eph; + space_node_t::TimeProperties t; + { + space_node_t::SatelliteProperties::Ephemeris::raw_t eph_raw = {0}; + space_node_t::TimeProperties::raw_t t_raw = {0}; + + eph_raw.N_T = t_raw.NA = 251; + t_raw.N_4 = 5; + + eph = (space_node_t::SatelliteProperties::Ephemeris)eph_raw; + t = (space_node_t::TimeProperties)t_raw; + + eph.t_b = 11700; + eph.xn = 7003.008789E3; eph.yn = -12206.626953E3; eph.zn = 21280.765625E3; + eph.xn_dot = 0.7835417E3; eph.yn_dot = 2.8042530E3; eph.zn_dot = 1.3525150E3; + eph.xn_ddot = 0; eph.yn_ddot = 1.7E-6; eph.zn_ddot = -5.41E-6; + } + + space_node_t::SatelliteProperties::Ephemeris_with_Time eph_t(eph, t); + + space_node_t::SatelliteProperties::Ephemeris::constellation_t pos_vel( + eph_t.calculate_constellation(12300 - 11700)); + + // precise (J.1) + // 7523.174819 km, -10506.961965 km, 21999.239413 km + // 0.950126007 km/s, 2.855687825 km/s, 1.040679862 km/s + // Jx0m = -5.035590E-10 km/s2, Jy0m = 7.379024E-10 km/s2, Jz0m = -1.648033E-9 km/s2 + // Jx0s = 4.428827E-10 km/s2, Jy0s = 3.541631E-10 km/s2, Jz0s = -8.911601E-10 km/s2 + + // simplified (J.2) + // 7523.174853 km, -10506.962176 km, 21999.239866 km + // 0.95012609 km/s, 2.85568710 km/s, 1.04068137 km/s + + BOOST_REQUIRE_CLOSE(pos_vel.position[0], 7523.174819E3, 1E0); // 1m + BOOST_REQUIRE_CLOSE(pos_vel.position[1], -10506.961965E3, 1E0); + BOOST_REQUIRE_CLOSE(pos_vel.position[2], 21999.239413E3, 1E0); + + BOOST_REQUIRE_CLOSE(pos_vel.velocity[0], 0.950126007E3, 1E-1); // 0.1m/s + BOOST_REQUIRE_CLOSE(pos_vel.velocity[1], 2.855687825E3, 1E-1); + BOOST_REQUIRE_CLOSE(pos_vel.velocity[2], 1.040679862E3, 1E-1); +} + BOOST_AUTO_TEST_SUITE_END() From 22ddf8a7dadc70195f97eae16416f617efee2e06 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 29 Dec 2021 09:33:37 +0900 Subject: [PATCH 13/59] Add GLONASS NAV converter to RINEX reader/writer --- tool/navigation/RINEX.h | 301 ++++++++++++++++++++++++++++++++++++++- tool/test/test_RINEX.cpp | 158 ++++++++++++++++++++ 2 files changed, 455 insertions(+), 4 deletions(-) diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index d8362e5ab..15ae1426b 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -54,6 +54,7 @@ #include "util/text_helper.h" #include "GPS.h" +#include "GLONASS.h" template class RINEX_Reader { @@ -375,6 +376,47 @@ struct RINEX_NAV { eph.fit_interval = ((fit_interval_hr < 4) ? 4 : fit_interval_hr) * 60 * 60; } }; + struct message_glonass_t { + typedef typename GLONASS_SpaceNode + ::SatelliteProperties::Ephemeris_with_Time eph_t; + int svid; + std::tm date_tm; + int t_year4, t_year2, t_mon12; + FloatT t_sec; + FloatT tau_n_neg, gamma_n; + unsigned int t_k; + FloatT x_km, dx_km_s, ddx_km_s2; + FloatT y_km, dy_km_s, ddy_km_s2; + FloatT z_km, dz_km_s, ddz_km_s2; + unsigned int B_n, E_n; + int freq_num; // 1-24(ver.2), -7-13(ver.3) + message_glonass_t() {} + message_glonass_t(const eph_t &eph) + : svid((int)eph.svid), + date_tm(eph.c_tm_utc()), + t_year4(date_tm.tm_year + 1900), + t_year2(date_tm.tm_year % 100), + t_mon12(date_tm.tm_mon + 1), + t_sec(date_tm.tm_sec), + tau_n_neg(-eph.tau_n), gamma_n(eph.gamma_n), t_k(eph.t_k), + x_km(1E-3 * eph.xn), dx_km_s(1E-3 * eph.xn_dot), ddx_km_s2(1E-3 * eph.xn_ddot), + y_km(1E-3 * eph.yn), dy_km_s(1E-3 * eph.yn_dot), ddy_km_s2(1E-3 * eph.yn_ddot), + z_km(1E-3 * eph.zn), dz_km_s(1E-3 * eph.zn_dot), ddz_km_s2(1E-3 * eph.zn_ddot), + B_n(eph.B_n), E_n(eph.E_n), + freq_num(eph.freq_ch) { + } + operator eph_t() const { + typename GLONASS_SpaceNode::SatelliteProperties::Ephemeris eph = {0}; + eph.svid = (unsigned int)svid; + eph.freq_ch = freq_num; + eph.tau_n = -tau_n_neg; eph.gamma_n = gamma_n; eph.t_k = t_k; + eph.xn = 1E3 * x_km; eph.xn_dot = 1E3 * dx_km_s; eph.xn_ddot = 1E3 * ddx_km_s2; + eph.yn = 1E3 * y_km; eph.yn_dot = 1E3 * dy_km_s; eph.yn_ddot = 1E3 * ddy_km_s2; + eph.zn = 1E3 * z_km; eph.zn_dot = 1E3 * dz_km_s; eph.zn_ddot = 1E3 * ddz_km_s2; + eph.B_n = B_n; eph.E_n = E_n; + return eph_t(eph, date_tm); + } + }; }; template @@ -385,6 +427,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { public: typedef typename RINEX_NAV::space_node_t space_node_t; typedef typename RINEX_NAV::message_t message_t; + typedef typename RINEX_NAV::message_glonass_t message_glonass_t; typedef typename space_node_t::Ionospheric_UTC_Parameters iono_utc_t; static const typename super_t::convert_item_t eph0_v2[10], eph0_v3[10]; @@ -396,10 +439,16 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { static const typename super_t::convert_item_t eph6_v2[4], eph6_v3[4]; static const typename super_t::convert_item_t eph7_v2[2], eph7_v3[2]; + static const typename super_t::convert_item_t eph0_glonass_v2[10], eph0_glonass_v3[10]; + static const typename super_t::convert_item_t eph1_glonass_v2[4], eph1_glonass_v3[4]; + static const typename super_t::convert_item_t eph2_glonass_v2[4], eph2_glonass_v3[4]; + static const typename super_t::convert_item_t eph3_glonass_v2[4], eph3_glonass_v3[4]; + protected: typename super_t::version_type_t::sat_system_t sys_of_msg; message_t msg; - + message_glonass_t msg_glonass; + void seek_next_v2_gps() { char buf[256]; @@ -436,9 +485,27 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { for(int i(0); i < 4; i++){ if((!super_t::src.good()) || super_t::src.getline(buf, sizeof(buf)).fail()){return;} + std::string line(buf); + + switch(i){ + case 0: { + super_t::convert(eph0_glonass_v2, line, &msg_glonass); + msg_glonass.date_tm.tm_year = msg_glonass.t_year2 + (msg_glonass.t_year2 < 80 ? 100 : 0); // greater than 1980 + msg_glonass.date_tm.tm_mon = msg_glonass.t_mon12 - 1; // month [0, 11] + msg_glonass.date_tm.tm_sec = (int)msg_glonass.t_sec; + break; + } + case 1: super_t::convert(eph1_glonass_v2, line, &msg_glonass); break; + case 2: + super_t::convert(eph2_glonass_v2, line, &msg_glonass); + if(super_t::version_type.version < 211){ + //msg_glonass.freq_num; // TODO 1..24? convert to value ranging from -7 to 6? + } + break; + case 3: super_t::convert(eph3_glonass_v2, line, &msg_glonass); break; + } } - //sys_of_msg = super_t::version_type_t::SYS_GLONASS; // TODO currently not implemented - sys_of_msg = super_t::version_type_t::SYS_UNKNOWN; + sys_of_msg = super_t::version_type_t::SYS_GLONASS; super_t::_has_next = true; } @@ -477,6 +544,28 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { super_t::_has_next = true; } + template + void seek_next_v3_glonass(char (&buf)[N]) { + super_t::convert(eph0_glonass_v3, std::string(buf), &msg_glonass); + msg_glonass.date_tm.tm_year = msg_glonass.t_year4 - 1900; // tm_year base is 1900 + msg_glonass.date_tm.tm_mon = msg_glonass.t_mon12 - 1; // month [0, 11] + msg_glonass.t_sec = msg_glonass.date_tm.tm_sec; + + for(int i(1); i < 4; i++){ + if((!super_t::src.good()) + || super_t::src.getline(buf, sizeof(buf)).fail()){return;} + std::string line(buf); + + switch(i){ + case 1: super_t::convert(eph1_glonass_v3, line, &msg_glonass); break; + case 2: super_t::convert(eph2_glonass_v3, line, &msg_glonass); break; + case 3: super_t::convert(eph3_glonass_v3, line, &msg_glonass); break; + } + } + sys_of_msg = super_t::version_type_t::SYS_GLONASS; + super_t::_has_next = true; + } + template void seek_next_v3_not_implemented(char (&buf)[N], const int &lines) { for(int i(1); i < lines; i++){ @@ -496,7 +585,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { switch(buf[0]){ case 'G': seek_next_v3_gps(buf); return; // GPS case 'E': seek_next_v3_not_implemented(buf, 8); return; // Galileo - case 'R': seek_next_v3_not_implemented(buf, 4); return; // Glonass + case 'R': seek_next_v3_glonass(buf); return; // Glonass case 'J': seek_next_v3_not_implemented(buf, 8); return; // QZSS case 'C': seek_next_v3_not_implemented(buf, 8); return; // Beido case 'S': seek_next_v3_not_implemented(buf, 4); return; // SBAS @@ -604,8 +693,64 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { return alpha && beta && utc && leap; } + struct t_corr_glonass_t { + int year, month, day; + FloatT tau_c_neg; + int leap_sec; + }; + static const typename super_t::convert_item_t t_corr_glonass_v2[4]; + + bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const { + bool utc, leap; + super_t::header_t::const_iterator it; + + if(utc = ((it = _header.find("CORR TO SYSTEM TIME")) != _header.end())){ + super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass); + } + + if(leap = ((it = _header.find("LEAP SECONDS")) != _header.end())){ + iono_utc_t iono_utc; + super_t::convert(utc_leap_v2, it->second.front(), &iono_utc); + t_corr_glonass.leap_sec = iono_utc.delta_t_LS; + } + + return utc && leap; + } + + bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const { + iono_utc_t iono_utc; + bool utc(false), leap(false); + typedef super_t::header_t::const_iterator it_t; + typedef super_t::header_t::mapped_type::const_iterator it2_t; + + it_t it; + + if((it = _header.find("TIME SYSTEM CORR")) != _header.end()){ + for(it2_t it2(it->second.begin()), it2_end(it->second.end()); it2 != it2_end; ++it2){ + if(it2->find("GLUT") == it2->npos){continue;} + super_t::convert(utc_v3, *it2, &iono_utc); + t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0; + t_corr_glonass.tau_c_neg = iono_utc.A0; + utc = true; + } + } + + if((it = _header.find("LEAP SECONDS")) != _header.end()){ + if(version_type.version >= 301){ + super_t::convert(utc_leap_v301, it->second.front(), &iono_utc); + }else{ + super_t::convert(utc_leap_v2, it->second.front(), &iono_utc); + } + t_corr_glonass.leap_sec = iono_utc.delta_t_LS; + leap = true; + } + + return utc && leap; + } + struct space_node_list_t { space_node_t *gps; + GLONASS_SpaceNode *glonass; }; static int read_all(std::istream &in, space_node_list_t &space_nodes = {0}){ @@ -618,6 +763,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { ? reader.extract_iono_utc_v3(*space_nodes.gps) : reader.extract_iono_utc_v2(*space_nodes.gps); } + t_corr_glonass_t t_corr_glonass = {0}; + if(space_nodes.glonass){ + (reader.version_type.version >= 300) + ? reader.extract_t_corr_glonass_v3(t_corr_glonass) + : reader.extract_t_corr_glonass_v2(t_corr_glonass); + } int res(0); for(; reader.has_next(); reader.next()){ switch(reader.sys_of_msg){ @@ -626,6 +777,16 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { space_nodes.gps->satellite(reader.msg.eph.svid).register_ephemeris(reader.msg.eph); res++; break; + case super_t::version_type_t::SYS_GLONASS: { + if(!space_nodes.glonass){break;} + typename message_glonass_t::eph_t eph0(reader.msg_glonass); + eph0.tau_c = -t_corr_glonass.tau_c_neg; + typename GLONASS_SpaceNode + ::SatelliteProperties::Ephemeris_with_GPS_Time eph(eph0, t_corr_glonass.leap_sec); + space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph); + res++; + break; + } default: break; } } @@ -1096,6 +1257,78 @@ const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader GEN_E(23, 19, 12, message_t, fit_interval_hr), }; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph0_glonass_v2[] = { + GEN_D( 0, 2, message_glonass_t, svid, int), + GEN_D( 3, 2, message_glonass_t, t_year2, int), + GEN_D( 6, 2, message_glonass_t, t_mon12, int), + GEN_D( 9, 2, message_glonass_t, date_tm.tm_mday, int), + GEN_D(12, 2, message_glonass_t, date_tm.tm_hour, int), + GEN_D(15, 2, message_glonass_t, date_tm.tm_min, int), + GEN_F(17, 5, 1, message_glonass_t, t_sec), + GEN_E(22, 19, 12, message_glonass_t, tau_n_neg), + GEN_E(41, 19, 12, message_glonass_t, gamma_n), + GEN_E2(60, 19, 12, message_glonass_t, t_k, unsigned int), +}; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph0_glonass_v3[] = { + GEN_I( 1, 2, message_glonass_t, svid, int), + GEN_I( 4, 4, message_glonass_t, t_year4, int), + GEN_I( 9, 2, message_glonass_t, t_mon12, int), + GEN_I(12, 2, message_glonass_t, date_tm.tm_mday, int), + GEN_I(15, 2, message_glonass_t, date_tm.tm_hour, int), + GEN_I(18, 2, message_glonass_t, date_tm.tm_min, int), + GEN_I(21, 2, message_glonass_t, date_tm.tm_sec, int), + GEN_E(23, 19, 12, message_glonass_t, tau_n_neg), + GEN_E(42, 19, 12, message_glonass_t, gamma_n), + GEN_E2(61, 19, 12, message_glonass_t, t_k, unsigned int), +}; + +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph1_glonass_v2[] = { + GEN_E( 3, 19, 12, message_glonass_t, x_km), + GEN_E(22, 19, 12, message_glonass_t, dx_km_s), + GEN_E(41, 19, 12, message_glonass_t, ddx_km_s2), + GEN_E2(60, 19, 12, message_glonass_t, B_n, unsigned int), +}; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph1_glonass_v3[] = { + GEN_E( 4, 19, 12, message_glonass_t, x_km), + GEN_E(23, 19, 12, message_glonass_t, dx_km_s), + GEN_E(42, 19, 12, message_glonass_t, ddx_km_s2), + GEN_E2(61, 19, 12, message_glonass_t, B_n, unsigned int), +}; + +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph2_glonass_v2[] = { + GEN_E( 3, 19, 12, message_glonass_t, y_km), + GEN_E(22, 19, 12, message_glonass_t, dy_km_s), + GEN_E(41, 19, 12, message_glonass_t, ddy_km_s2), + GEN_E2(60, 19, 12, message_glonass_t, freq_num, int), +}; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph2_glonass_v3[] = { + GEN_E( 4, 19, 12, message_glonass_t, y_km), + GEN_E(23, 19, 12, message_glonass_t, dy_km_s), + GEN_E(42, 19, 12, message_glonass_t, ddy_km_s2), + GEN_E2(61, 19, 12, message_glonass_t, freq_num, int), +}; + +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph3_glonass_v2[] = { + GEN_E( 3, 19, 12, message_glonass_t, z_km), + GEN_E(22, 19, 12, message_glonass_t, dz_km_s), + GEN_E(41, 19, 12, message_glonass_t, ddz_km_s2), + GEN_E2(60, 19, 12, message_glonass_t, E_n, unsigned int), +}; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph3_glonass_v3[] = { + GEN_E( 4, 19, 12, message_glonass_t, z_km), + GEN_E(23, 19, 12, message_glonass_t, dz_km_s), + GEN_E(42, 19, 12, message_glonass_t, ddz_km_s2), + GEN_E2(61, 19, 12, message_glonass_t, E_n, unsigned int), +}; + template const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::iono_alpha_v2[] = { GEN_E( 2, 12, 4, iono_utc_t, alpha[0]), @@ -1158,6 +1391,14 @@ const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader GEN_D(18, 6, iono_utc_t, DN, int), }; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::t_corr_glonass_v2[] = { + GEN_D( 0, 6, t_corr_glonass_t, year, int), + GEN_D( 6, 6, t_corr_glonass_t, month, int), + GEN_D(12, 6, t_corr_glonass_t, day, int), + GEN_E(21, 19, 12, t_corr_glonass_t, tau_c_neg), +}; + template const typename RINEX_OBS_Reader::convert_item_t RINEX_OBS_Reader::epoch_flag_v2[] = { GEN_I( 1, 2, epoch_flag_t, epoch_year2, int), @@ -1384,6 +1625,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { public: typedef typename RINEX_NAV::space_node_t space_node_t; typedef typename RINEX_NAV::message_t message_t; + typedef typename RINEX_NAV::message_glonass_t message_glonass_t; static const typename super_t::header_item_t default_header[]; static const int default_header_size; @@ -1481,6 +1723,42 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { dist << buf.str(); return *this; } + self_t &operator<<(const message_glonass_t &msg){ + std::stringstream buf; + switch(super_t::_version_type.version / 100){ + case 2: + for(int i(0); i < 4; ++i){ + std::string s(80, ' '); + switch(i){ + case 0: super_t::convert(reader_t::eph0_glonass_v2, s, &msg); break; + case 1: super_t::convert(reader_t::eph1_glonass_v2, s, &msg); break; + case 2: + if(super_t::_version_type.version < 211){ + //msg_glonass.freq_num; // TODO convert to value 1..24? + } + super_t::convert(reader_t::eph2_glonass_v2, s, &msg); + break; + case 3: super_t::convert(reader_t::eph3_glonass_v2, s, &msg); break; + } + buf << s << std::endl; + } + break; + case 3: + for(int i(0); i < 4; ++i){ + std::string s(80, ' '); + switch(i){ + case 0: super_t::convert(reader_t::eph0_glonass_v3, s, &msg); s[0] = 'R'; break; + case 1: super_t::convert(reader_t::eph1_glonass_v3, s, &msg); break; + case 2: super_t::convert(reader_t::eph2_glonass_v3, s, &msg); break; + case 3: super_t::convert(reader_t::eph3_glonass_v3, s, &msg); break; + } + buf << s << std::endl; + } + break; + } + dist << buf.str(); + return *this; + } public: void set_version( @@ -1492,6 +1770,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { struct space_node_list_t { const space_node_t *gps; + GLONASS_SpaceNode *glonass; }; int write_all( const space_node_list_t &space_nodes, @@ -1532,6 +1811,10 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { w << message_t(eph); counter++; } + void operator()(const typename message_glonass_t::eph_t &eph) { + w << message_glonass_t(eph); + counter++; + } } functor = {*this, res}; if(space_nodes.gps){ for(typename space_node_t::satellites_t::const_iterator @@ -1542,6 +1825,16 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { space_node_t::Satellite::eph_list_t::EACH_ALL_INVERTED); } } + if(space_nodes.glonass){ + for(typename GLONASS_SpaceNode::satellites_t::const_iterator + it(space_nodes.glonass->satellites().begin()), + it_end(space_nodes.glonass->satellites().end()); + it != it_end; ++it){ + it->second.each_ephemeris( + functor, + GLONASS_SpaceNode::Satellite::eph_list_t::EACH_ALL_INVERTED); + } + } return res; } static int write_all( diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index d86a9a82f..066ef1e1b 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -1,9 +1,13 @@ +#if defined(_MSC_VER) +#define _USE_MATH_DEFINES +#endif #include #include #include #include "navigation/RINEX.h" #include "navigation/GPS.h" +#include "navigation/GLONASS.h" #include #include @@ -20,6 +24,7 @@ using boost::format; typedef double fnum_t; typedef GPS_SpaceNode gps_t; +typedef GLONASS_SpaceNode glonass_t; BOOST_AUTO_TEST_SUITE(RINEX) @@ -259,6 +264,159 @@ BOOST_AUTO_TEST_CASE(nav_GPS_v3){ compare_lines(src, dist); } +BOOST_AUTO_TEST_CASE(nav_GLONASS_v2){ + const char *src = \ + " 2.11 GLONASS NAV DATA RINEX VERSION / TYPE\n" + "ASRINEXG V1.1.0 VM AIUB 19-FEB-98 10:42 PGM / RUN BY / DATE \n" + "STATION ZIMMERWALD COMMENT \n" + " 1998 2 16 0.379979610443D-06 CORR TO SYSTEM TIME \n" + " END OF HEADER \n" + " 3 98 2 15 0 15 0.0 0.163525342941D-03 0.363797880709D-11 0.108000000000D+05 \n" + " 0.106275903320D+05-0.348924636841D+00 0.931322574615D-09 0.000000000000D+00 \n" + " -0.944422070313D+04 0.288163375854D+01 0.931322574615D-09 0.210000000000D+02 \n" + " 0.212257280273D+05 0.144599342346D+01-0.186264514923D-08 0.300000000000D+01 \n"; + //----|---1|0---|---2|0---|---3|0---|---4|0---|---5|0---|---6|0---|---7|0---|---8| + + typedef RINEX_NAV_Reader reader_t; + typedef RINEX_NAV_Writer writer_t; + + check_reader_versatility_to_input(src); + + gps_t gps; + glonass_t glonass; + { + std::stringbuf sbuf(src); + std::istream in(&sbuf); + reader_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + reader_t::read_all(in, space_nodes); + } + + { + std::tm t_tm = {0, 15, 0, 15, 2 - 1, 1998 - 1900}; + /*typename*/ gps_t::gps_time_t t(t_tm); + glonass.satellite(3).select_ephemeris(t); + const /*typename*/ glonass_t::Satellite::Ephemeris &eph(glonass.satellite(3).ephemeris()); + + BOOST_CHECK_SMALL(std::abs(-.163525342941E-03 - eph.tau_n), 1E-15); // tau_n is inverted + BOOST_CHECK_SMALL(std::abs( .363797880709E-11 - eph.gamma_n), 1E-23); + BOOST_CHECK_SMALL(std::abs( .108000000000E+05 - eph.t_k), 1E-07); + + BOOST_CHECK_SMALL(std::abs( .106275903320E+08 - eph.xn), 1E-04); + BOOST_CHECK_SMALL(std::abs(-.348924636841E+03 - eph.xn_dot), 1E-09); + BOOST_CHECK_SMALL(std::abs( .931322574615E-06 - eph.xn_ddot), 1E-18); + BOOST_CHECK_SMALL(std::abs( .000000000000E+00 - eph.B_n), 1E-12); + + BOOST_CHECK_SMALL(std::abs(-.944422070313E+07 - eph.yn), 1E-05); + BOOST_CHECK_SMALL(std::abs( .288163375854E+04 - eph.yn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs( .931322574615E-06 - eph.yn_ddot), 1E-18); + BOOST_CHECK_SMALL(std::abs( .210000000000E+02 - eph.freq_ch), 1E-08); + + BOOST_CHECK_SMALL(std::abs( .212257280273E+08 - eph.zn), 1E-04); + BOOST_CHECK_SMALL(std::abs( .144599342346E+04 - eph.zn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs(-.186264514923E-05 - eph.zn_ddot), 1E-17); + BOOST_CHECK_SMALL(std::abs( .300000000000E+01 - eph.E_n), 1E-11); + } + + std::string dist; + { + std::stringstream ss; + writer_t writer(ss); + writer.header()["PGM / RUN BY / DATE"] + = "ASRINEXG V1.1.0 VM AIUB 19-FEB-98 10:42"; + writer.header()["COMMENT"] = "STATION ZIMMERWALD"; + writer.header()["CORR TO SYSTEM TIME"] << " 1998 2 16 0.379979610443D-06"; + + writer_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + writer.write_all(space_nodes, 211); + dist = ss.str(); + } + + //compare_lines(src, dist, 5); // TODO check header +} + +BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ + const char *src = \ + " 3.04 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE\n" + "XXRINEXN V3 AIUB 20061002 000123 UTC PGM / RUN BY / DATE \n" + "EXAMPLE OF VERSION 3.04 FORMAT COMMENT \n" + "GPSA 0.1025E-07 0.7451E-08 -0.5960E-07 -0.5960E-07 IONOSPHERIC CORR \n" + "GPSB 0.8806E+05 0.0000E+00 -0.1966E+06 -0.6554E+05 IONOSPHERIC CORR \n" + "GPUT 0.2793967723E-08 0.000000000E+00 147456 1395 G10 2 TIME SYSTEM CORR \n" + "GLUT 0.7823109626E-06 0.000000000E+00 0 1395 R10 0 TIME SYSTEM CORR \n" + " 14 LEAP SECONDS \n" + " END OF HEADER \n" + "R01 2006 10 01 00 15 00-0.137668102980E-04-0.454747350886E-11 0.900000000000E+02\n" + " 0.157594921875E+05-0.145566368103E+01 0.000000000000E+00 0.000000000000E+00\n" + " -0.813711474609E+04 0.205006790161E+01 0.931322574615E-09 0.700000000000E+01\n" + " 0.183413398438E+05 0.215388488770E+01-0.186264514923E-08 0.100000000000E+01\n"; + //----|---1|0---|---2|0---|---3|0---|---4|0---|---5|0---|---6|0---|---7|0---|---8| + + typedef RINEX_NAV_Reader reader_t; + typedef RINEX_NAV_Writer writer_t; + + check_reader_versatility_to_input(src); + + gps_t gps; + glonass_t glonass; + { + std::stringbuf sbuf(src); + std::istream in(&sbuf); + reader_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + reader_t::read_all(in, space_nodes); + } + + { + std::tm t_oc_tm = {0, 15, 0, 1, 10 - 1, 2006 - 1900}; + /*typename*/ gps_t::gps_time_t t_oc(t_oc_tm); + glonass.satellite(1).select_ephemeris(t_oc); + const /*typename*/ glonass_t::Satellite::Ephemeris &eph(glonass.satellite(1).ephemeris()); + + BOOST_CHECK_SMALL(std::abs( .137668102980E-04 - eph.tau_n), 1E-16); // tau_n is inverted + BOOST_CHECK_SMALL(std::abs(-.454747350886E-11 - eph.gamma_n), 1E-23); + BOOST_CHECK_SMALL(std::abs( .900000000000E+02 - eph.t_k), 1E-10); + + BOOST_CHECK_SMALL(std::abs( .157594921875E+08 - eph.xn), 1E-04); + BOOST_CHECK_SMALL(std::abs(-.145566368103E+04 - eph.xn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs( .000000000000E+03 - eph.xn_ddot), 1E-09); + BOOST_CHECK_SMALL(std::abs( .000000000000E+00 - eph.B_n), 1E-12); + + BOOST_CHECK_SMALL(std::abs(-.813711474609E+07 - eph.yn), 1E-05); + BOOST_CHECK_SMALL(std::abs( .205006790161E+04 - eph.yn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs( .931322574615E-06 - eph.yn_ddot), 1E-18); + BOOST_CHECK_SMALL(std::abs( .700000000000E+01 - eph.freq_ch), 1E-11); + + BOOST_CHECK_SMALL(std::abs( .183413398438E+08 - eph.zn), 1E-04); + BOOST_CHECK_SMALL(std::abs( .215388488770E+04 - eph.zn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs(-.186264514923E-05 - eph.zn_ddot), 1E-17); + BOOST_CHECK_SMALL(std::abs( .100000000000E+01 - eph.E_n), 1E-11); + } + + std::string dist; + { + std::stringstream ss; + writer_t writer(ss); + //writer.header()["PGM / RUN BY / DATE"] + // = "XXRINEXN V3 AIUB 20061002 000123 UTC"; + std::tm t_header = {23, 1, 0, 2, 10 - 1, 2006 - 1900}; + writer.pgm_runby_date("XXRINEXN V3", "AIUB", t_header, "UTC"); + writer.header()["COMMENT"] << "EXAMPLE OF VERSION 3.04 FORMAT"; + writer.header()["TIME SYSTEM CORR"] + << "GPUT 0.2793967723E-08 0.000000000E+00 147456 1395 G10 2" //writer.utc_params(gps); + << "GLUT 0.7823109626E-06 0.000000000E+00 0 1395 R10 0"; + writer.header()["LEAP SECONDS"] = " 14"; // writer.leap_seconds(gps); + + writer_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + writer.write_all(space_nodes, 304); + dist = ss.str(); + } + + //compare_lines(src, dist, 9); // TODO check header +} + BOOST_AUTO_TEST_CASE(obs_GPS_v2){ const char *src = \ " 2.11 OBSERVATION DATA M (MIXED) RINEX VERSION / TYPE\n" From 1931a69d6bb5935a2a51fd3d71fb62f4abeb2f6e Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 12 Jan 2022 11:41:16 +0900 Subject: [PATCH 14/59] Add GLONASS_SpaceNode::latest_ephemeris() --- tool/navigation/GLONASS.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index ee45cfd65..74d1ddb5f 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -1058,6 +1058,20 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} it->second.select_ephemeris(target_time); } } + typename Satellite::eph_t latest_ephemeris() const { + struct { + typename Satellite::eph_t res; + void operator()(const typename Satellite::eph_t &eph){ + if(res.t_b_gps < eph.t_b_gps){res = eph;} + } + } functor; + for(typename satellites_t::const_iterator + it(_satellites.begin()), it_end(_satellites.end()); + it != it_end; ++it){ + it->second.each_ephemeris(functor, Satellite::eph_list_t::EACH_NO_REDUNDANT); + } + return functor.res; + } }; template From 246ba849dfb7984e892d0e2fb77dd37cf8cd738c Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 12 Jan 2022 09:03:34 +0900 Subject: [PATCH 15/59] Add time correction and leap seconds to GLONASS RINEX writer --- tool/navigation/RINEX.h | 41 ++++++++++++++++++++++++++++++++++++++++ tool/test/test_RINEX.cpp | 9 +++++---- 2 files changed, 46 insertions(+), 4 deletions(-) diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 15ae1426b..84aca0c1c 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -1798,6 +1798,47 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { break; } }while(false); + while(space_nodes.glonass){ + ++systems; + set_version(version, super_t::version_type_t::SYS_GLONASS); + typename GLONASS_SpaceNode::Satellite::eph_t latest( + space_nodes.glonass->latest_ephemeris()); + if(latest.t_b_gps.week <= 0){break;} + typename reader_t::iono_utc_t iono_utc = {0}; + iono_utc.A0 = -latest.tau_c; + iono_utc.t_ot = latest.t_b_gps.seconds; + iono_utc.WN_t = latest.t_b_gps.week; + iono_utc.delta_t_LS = (int)std::floor(0.5 + + typename space_node_t::gps_time_t(latest.c_tm_utc()).interval(latest.t_b_gps)); + switch(version / 100){ + case 2: + if(_header["CORR TO SYSTEM TIME"].entries() == 0){ + std::tm t_tm(typename space_node_t::gps_time_t(iono_utc.WN_t, iono_utc.t_ot).c_tm()); + typename reader_t::t_corr_glonass_t t_corr_glonass = { + t_tm.tm_year + 1900, t_tm.tm_mon + 1, t_tm.tm_mday, // year, month, day + iono_utc.A0, // tau_c_neg + }; + std::string s(60, ' '); + super_t::convert(reader_t::t_corr_glonass_v2, s, &t_corr_glonass); + _header["CORR TO SYSTEM TIME"] = s; + } + break; + case 3: + if(_header["TIME SYSTEM CORR"].find("GLUT") == _header.end()){ + std::string s(60, ' '); + super_t::convert(reader_t::utc_v3, s, &iono_utc); + _header["TIME SYSTEM CORR"] << s.replace(0, 4, "GLUT", 4); + } + break; + } + if((_header["LEAP SECONDS"].entries() == 0) && (iono_utc.delta_t_LS != 0)){ + // ver.3 can use ver.2 format with blank fields + std::string s(60, ' '); + super_t::convert(reader_t::utc_leap_v2, s, &iono_utc); + _header["LEAP SECONDS"] = s; + } + break; + } if(systems > 1){ set_version(version, super_t::version_type_t::SYS_MIXED); } diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index 066ef1e1b..033d27fcd 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -282,12 +282,11 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v2){ check_reader_versatility_to_input(src); - gps_t gps; glonass_t glonass; { std::stringbuf sbuf(src); std::istream in(&sbuf); - reader_t::space_node_list_t space_nodes = {&gps}; + reader_t::space_node_list_t space_nodes = {NULL}; space_nodes.glonass = &glonass; reader_t::read_all(in, space_nodes); } @@ -325,9 +324,9 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v2){ writer.header()["PGM / RUN BY / DATE"] = "ASRINEXG V1.1.0 VM AIUB 19-FEB-98 10:42"; writer.header()["COMMENT"] = "STATION ZIMMERWALD"; - writer.header()["CORR TO SYSTEM TIME"] << " 1998 2 16 0.379979610443D-06"; + //writer.header()["CORR TO SYSTEM TIME"] << " 1998 2 16 0.379979610443D-06"; - writer_t::space_node_list_t space_nodes = {&gps}; + writer_t::space_node_list_t space_nodes = {NULL}; space_nodes.glonass = &glonass; writer.write_all(space_nodes, 211); dist = ss.str(); @@ -403,10 +402,12 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ std::tm t_header = {23, 1, 0, 2, 10 - 1, 2006 - 1900}; writer.pgm_runby_date("XXRINEXN V3", "AIUB", t_header, "UTC"); writer.header()["COMMENT"] << "EXAMPLE OF VERSION 3.04 FORMAT"; +#if 0 writer.header()["TIME SYSTEM CORR"] << "GPUT 0.2793967723E-08 0.000000000E+00 147456 1395 G10 2" //writer.utc_params(gps); << "GLUT 0.7823109626E-06 0.000000000E+00 0 1395 R10 0"; writer.header()["LEAP SECONDS"] = " 14"; // writer.leap_seconds(gps); +#endif writer_t::space_node_list_t space_nodes = {&gps}; space_nodes.glonass = &glonass; From f1914241bcb4e2f981a4bbe5d56e19fd6b34d40a Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 12 Jan 2022 13:53:47 +0900 Subject: [PATCH 16/59] Add provisional tau_GPS reader/writer --- tool/navigation/RINEX.h | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 84aca0c1c..0fd20223b 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -695,7 +695,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { struct t_corr_glonass_t { int year, month, day; - FloatT tau_c_neg; + FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity int leap_sec; }; static const typename super_t::convert_item_t t_corr_glonass_v2[4]; @@ -727,10 +727,14 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { if((it = _header.find("TIME SYSTEM CORR")) != _header.end()){ for(it2_t it2(it->second.begin()), it2_end(it->second.end()); it2 != it2_end; ++it2){ - if(it2->find("GLUT") == it2->npos){continue;} - super_t::convert(utc_v3, *it2, &iono_utc); - t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0; - t_corr_glonass.tau_c_neg = iono_utc.A0; + if(it2->find("GLUT") != it2->npos){ + super_t::convert(utc_v3, *it2, &iono_utc); + t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0; + t_corr_glonass.tau_c_neg = iono_utc.A0; + }else if(it2->find("GLGP") != it2->npos){ + super_t::convert(utc_v3, *it2, &iono_utc); + t_corr_glonass.tau_GPS = iono_utc.A0; + } utc = true; } } @@ -781,6 +785,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { if(!space_nodes.glonass){break;} typename message_glonass_t::eph_t eph0(reader.msg_glonass); eph0.tau_c = -t_corr_glonass.tau_c_neg; + eph0.tau_GPS = t_corr_glonass.tau_GPS; typename GLONASS_SpaceNode ::SatelliteProperties::Ephemeris_with_GPS_Time eph(eph0, t_corr_glonass.leap_sec); space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph); @@ -1805,18 +1810,17 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { space_nodes.glonass->latest_ephemeris()); if(latest.t_b_gps.week <= 0){break;} typename reader_t::iono_utc_t iono_utc = {0}; - iono_utc.A0 = -latest.tau_c; iono_utc.t_ot = latest.t_b_gps.seconds; iono_utc.WN_t = latest.t_b_gps.week; iono_utc.delta_t_LS = (int)std::floor(0.5 + typename space_node_t::gps_time_t(latest.c_tm_utc()).interval(latest.t_b_gps)); switch(version / 100){ case 2: - if(_header["CORR TO SYSTEM TIME"].entries() == 0){ + if((_header["CORR TO SYSTEM TIME"].entries() == 0) && (latest.tau_c != 0)){ std::tm t_tm(typename space_node_t::gps_time_t(iono_utc.WN_t, iono_utc.t_ot).c_tm()); typename reader_t::t_corr_glonass_t t_corr_glonass = { t_tm.tm_year + 1900, t_tm.tm_mon + 1, t_tm.tm_mday, // year, month, day - iono_utc.A0, // tau_c_neg + -latest.tau_c, }; std::string s(60, ' '); super_t::convert(reader_t::t_corr_glonass_v2, s, &t_corr_glonass); @@ -1824,11 +1828,18 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { } break; case 3: - if(_header["TIME SYSTEM CORR"].find("GLUT") == _header.end()){ + if((_header["TIME SYSTEM CORR"].find("GLUT") == _header.end()) && (latest.tau_c != 0)){ std::string s(60, ' '); + iono_utc.A0 = -latest.tau_c; super_t::convert(reader_t::utc_v3, s, &iono_utc); _header["TIME SYSTEM CORR"] << s.replace(0, 4, "GLUT", 4); } + if((_header["TIME SYSTEM CORR"].find("GLGP") == _header.end()) && (latest.tau_GPS != 0)){ + std::string s(60, ' '); + iono_utc.A0 = latest.tau_GPS; + super_t::convert(reader_t::utc_v3, s, &iono_utc); + _header["TIME SYSTEM CORR"] << s.replace(0, 4, "GLGP", 4); + } break; } if((_header["LEAP SECONDS"].entries() == 0) && (iono_utc.delta_t_LS != 0)){ From 99c23d9ef65937b92d7dbfb3d519c8e580d6b8ea Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 13 Jan 2022 09:38:49 +0900 Subject: [PATCH 17/59] Update GLONASS constellation precise calculation (tentative) --- tool/navigation/GLONASS.h | 224 +++++++++++++++++++++++-------------- tool/test/test_GLONASS.cpp | 33 ++++++ 2 files changed, 172 insertions(+), 85 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 74d1ddb5f..6544d2b2b 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -444,55 +444,48 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} if(std::abs(E_k2 - E_k) < delta_limit){break;} E_k = E_k2; } - snu_k = std::sqrt(-e_k * e_k + 1) * std::sin(E_k) / (-e_k * std::cos(E_k) + 1); - cnu_k = (std::cos(E_k) - e_k) * std::sin(E_k) / (-e_k * std::cos(E_k) + 1); + float_t sE_k(std::sin(E_k)), cE_k(std::cos(E_k)), denom(-e_k * cE_k + 1); + snu_k = std::sqrt(-std::pow(e_k, 2) + 1) * sE_k / denom; + cnu_k = (cE_k - e_k) / denom; } }; struct lunar_solar_perturbations_t { - struct { + struct arg_t { float_t xi, eta, zeta, r; } m, s; - /** - * @param days Sum of days from the epoch at 00 hours Moscow Time (MT) on 1st January 1975 - * to the epoch at 00 hours MT of current date within which the instant t_e is. - * @param t_e reference time of ephemeris parameters (in Julian centuries of 36525 ephemeris days); - */ - lunar_solar_perturbations_t(const float_t &days, const float_t &t_e){ -#define dms2rad(deg, min, sec) \ -(M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg)) - static const float_t - a_m(3.84385243E8), // [m] - a_s(1.49598E11), //[m] - e_m(0.054900489), - e_s(0.016719), - i_m(dms2rad(5, 8, 43.4)), - epsilon(dms2rad(23, 26, 33)), - g_om(dms2rad(-63, 53, 43.41)), - g_1m(dms2rad(477198, 50, 56.79)), - Omega_om(dms2rad(259, 10, 59.79)), - Omega_1m(dms2rad(-1934, 8, 31.23)), - Gamma_o(dms2rad(-334, 19, 46.40)), - Gamma_1(dms2rad(4069, 2, 2.52)), - g_os(dms2rad(358, 28, 33.04)), - g_1s(dms2rad(0, 0, 129596579.10)); + struct constatnt_t { + float_t + a_m, a_s, // [m] + e_m, e_s, + i_m; + }; + struct var_t { + float_t + epsilon, + g_m, Omega_m, Gamma_m, + g_s, omega_s; + }; - static const float_t - sf_ci_m(-std::cos(i_m) + 1), si_m(std::sin(i_m)), - cepsilon(std::cos(epsilon)), sepsilon(std::sin(epsilon)); + static lunar_solar_perturbations_t setup( + const constatnt_t &C, const var_t &var){ - float_t T((27392.375 + days + t_e / 86400) / 36525); + lunar_solar_perturbations_t res; + + static const float_t + sf_ci_m(-std::cos(C.i_m) + 1), si_m(std::sin(C.i_m)), + cepsilon(std::cos(var.epsilon)), sepsilon(std::sin(var.epsilon)); - { // Eq.(3) lunar (m) - float_t Omega_m(Omega_om + Omega_1m * T); - float_t sOmega_m(std::sin(Omega_m)), cOmega_m(std::sin(Omega_m)); + { // ICD ver.4 and 5.1 Eq.(3) lunar (m) corresponding to Appendix.J.1 in CDMA ICD + // (ICD4) float_t Omega_m(Omega_om + Omega_1m * T); + float_t sOmega_m(std::sin(var.Omega_m)), cOmega_m(std::cos(var.Omega_m)); - float_t xi_ast(-cOmega_m * cOmega_m * sf_ci_m + 1); + float_t xi_ast(-std::pow(cOmega_m, 2) * sf_ci_m + 1); float_t eta_ast(sOmega_m * si_m); float_t zeta_ast(cOmega_m * si_m); float_t xi_11(sOmega_m * cOmega_m * sf_ci_m); - float_t xi_12(-sOmega_m * sOmega_m * sf_ci_m + 1); + float_t xi_12(-std::pow(sOmega_m, 2) * sf_ci_m + 1); float_t eta_11(xi_ast * cepsilon - zeta_ast * sepsilon); float_t eta_12(xi_11 * cepsilon + eta_ast * sepsilon); @@ -500,37 +493,104 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} float_t zeta_11(xi_ast * sepsilon + zeta_ast * cepsilon); float_t zeta_12(zeta_11 * sepsilon - eta_ast * cepsilon); - float_t Gamma(Gamma_o + Gamma_1 * T); - float_t sGamma(std::sin(Gamma)), cGamma(std::cos(Gamma)); + // (ICD4) float_t Gamma_m(Gamma_o + Gamma_1 * T); + float_t sGamma(std::sin(var.Gamma_m)), cGamma(std::cos(var.Gamma_m)); - eccentric_anomaly_t E_m(g_om + g_1m * T, e_m); + // (ICD4) eccentric_anomaly_t E_m(g_om + g_1m * T, C.e_m); + eccentric_anomaly_t E_m(var.g_m, C.e_m); float_t srad(E_m.snu_k * cGamma + E_m.cnu_k * sGamma), crad(E_m.cnu_k * cGamma - E_m.snu_k * sGamma); - m.xi = srad * xi_11 + crad * xi_12; - m.eta = srad * eta_11 + crad * eta_12; - m.zeta = srad * zeta_11 + crad * zeta_12; - m.r = a_m * (-e_m * std::cos(E_m.E_k) + 1); + res.m.xi = srad * xi_11 + crad * xi_12; + res.m.eta = srad * eta_11 + crad * eta_12; + res.m.zeta = srad * zeta_11 + crad * zeta_12; + res.m.r = C.a_m * (-C.e_m * std::cos(E_m.E_k) + 1); } - { // Eq.(3) solar (s) - float_t omega_s(dms2rad(281, 13, 15.00 + (6189.03 * T))); - float_t co(std::cos(omega_s)), so(std::sin(omega_s)); + { // ICD ver.4 and 5.1 Eq.(3) solar (s) corresponding to Appendix.S in CDMA ICD + // (ICD4) float_t omega_s(dms2rad(281, 13, (15.00 + (6189.03 * T)))); + float_t co(std::cos(var.omega_s)), so(std::sin(var.omega_s)); - eccentric_anomaly_t E_s(g_os + g_1s * T, e_s); + // (ICD4) eccentric_anomaly_t E_s(g_os + g_1s * T, C.e_s); + eccentric_anomaly_t E_s(var.g_s, C.e_s); float_t srad(E_s.snu_k * co + E_s.cnu_k * so), // = sin(nu_s + omega_s) crad(E_s.cnu_k * co - E_s.snu_k * so); // = cos(nu_s + omega_s) - s.xi = crad; - s.eta = srad * cepsilon; - s.zeta = srad * sepsilon; - s.r = a_s * (-e_s * std::cos(E_s.E_k) + 1); + res.s.xi = crad; + res.s.eta = srad * cepsilon; + res.s.zeta = srad * sepsilon; + res.s.r = C.a_s * (-C.e_s * std::cos(E_s.E_k) + 1); } + return res; + } + /** + * @param days Sum of days from the epoch at 00 hours Moscow Time (MT) on 1st January 1975 + * to the epoch at 00 hours MT of current date within which the instant t_e is. + * @param t_e reference time of ephemeris parameters (in Julian centuries of 36525 ephemeris days); + */ + static lunar_solar_perturbations_t base1975( + const float_t &days, const float_t &t_e) { +#define dms2rad(deg, min, sec) \ +(M_PI / 180 * ((float_t)sec / 3600 + (float_t)min / 60 + deg)) + static const constatnt_t C = { + 3.84385243E8, // a_m // [m] + 1.49598E11, // a_s // [m] + 0.054900489, // e_m + 0.016719, // e_s + dms2rad(5, 8, 43.4), // i_m // 0.08980398 rad + }; + static const float_t + epsilon(dms2rad(23, 26, 33)), + g_om(dms2rad(-63, 53, 43.41)), + g_1m(dms2rad(477198, 50, 56.79)), + Omega_om(dms2rad(259, 10, 59.79)), + Omega_1m(dms2rad(-1934, 8, 31.23)), + Gamma_o(dms2rad(-334, 19, 46.40)), + Gamma_1(dms2rad(4069, 2, 2.52)), + g_os(dms2rad(358, 28, 33.04)), + g_1s(dms2rad(0, 0, 129596579.10)); + + float_t T((27392.375 + days + t_e / 86400) / 36525); + // 27392.375 equals to interval days between 1900/1/1 0:0:0(GMT) to 1975/1/1 0:0:0(MT) + var_t var = { + epsilon, // epsilon + g_om + g_1m * T, // g_m + Omega_om + Omega_1m * T, // Omega_m + Gamma_o + Gamma_1 * T, // Gamma_m + g_os + g_1s * T, // g_s + dms2rad(281, 13, (15.00 + (6189.03 * T))), // omega_s + }; + return setup(C, var); #undef dms2rad } + static lunar_solar_perturbations_t base2000( + const float_t &jd0, const float_t &t_b) { + static const constatnt_t C = { + 3.84385243E8, // a_m // [m] + 1.49598E11, // a_s // [m] + 0.054900489, // e_m + 0.016719, // e_s + 0.0898041080, // i_m + }; + + float_t T((jd0 + ((t_b - 10800) / 86400) - 2451545.0) / 36525); + // 2451545.0 equals to Julian date for 2000/1/1 0:0:0(UTC) + float_t T2(std::pow(T, 2)); + + var_t var = { + 0.4090926006 - 0.0002270711 * T, // epsilon + 2.3555557435 + 8328.6914257190 * T + 0.0001545547 * T2, // g_m (q_m) + 2.1824391966 - 33.7570459536 * T + 0.0000362262 * T2, // Omega_m + 1.4547885346 + 71.0176852437 * T - 0.0001801481 * T2, // Gamma_m + 6.2400601269 + 628.3019551714 * T - 0.0000026820 * T2, // g_s (q_s) + -7.6281824375 + 0.0300101976 * T + 0.0000079741 * T2, // omega_s + }; + return setup(C, var); + } + }; struct differential_t { @@ -577,43 +637,37 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} struct differential2_t { lunar_solar_perturbations_t J_src; + static void equation2( + const float_t &mu, const typename lunar_solar_perturbations_t::arg_t &arg, + const float_t (&position)[3], + float_t (&J)[3]){ + // Eq.(2) + float_t + mu_bar(mu / std::pow(arg.r, 2)), + x_a_bar(position[0] / arg.r), + y_a_bar(position[1] / arg.r), + z_a_bar(position[2] / arg.r), + delta_x(arg.xi - x_a_bar), + delta_y(arg.eta - y_a_bar), + delta_z(arg.zeta - z_a_bar), + Delta3(std::pow( + std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5)); + J[0] = mu_bar * (delta_x / Delta3 - arg.xi); + J[1] = mu_bar * (delta_y / Delta3 - arg.eta); + J[2] = mu_bar * (delta_z / Delta3 - arg.zeta); + } + void calculate_Jm(const float_t (&position)[3], float_t (&J)[3]) const { + equation2(4902.835E9, J_src.m, position, J); // for lunar (m); mu in [m/s] + } + void calculate_Js(const float_t (&position)[3], float_t (&J)[3]) const { + equation2(0.1325263E21, J_src.s, position, J); // for solar (s); mu in [m/s] + } constellation_t operator()(const float_t &t, const constellation_t &x) const { // @see APPENDIX 3 EXAMPLES OF ALGORITHMS FOR CALCULATION OF COORDINATES AND VELOCITY - - float_t J_x_am, J_y_am, J_z_am; - { // Eq.(2) lunar (m) - float_t - mu_bar(4902.835E9 / std::pow(J_src.m.r, 2)), // mu_m in [m/s] - x_a_bar(x.position[0] / J_src.m.r), - y_a_bar(x.position[1] / J_src.m.r), - z_a_bar(x.position[2] / J_src.m.r), - delta_x(J_src.m.xi - x_a_bar), - delta_y(J_src.m.eta - y_a_bar), - delta_z(J_src.m.zeta - z_a_bar), - Delta3(std::pow( - std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5)); - J_x_am = mu_bar * (delta_x / Delta3 - J_src.m.xi); - J_y_am = mu_bar * (delta_y / Delta3 - J_src.m.eta); - J_z_am = mu_bar * (delta_z / Delta3 - J_src.m.zeta); - } - - float_t J_x_as, J_y_as, J_z_as; - { // Eq.(2) solar (s) - float_t - mu_bar(0.1325263E21 / std::pow(J_src.s.r, 2)), // mu_s in [m/s] - x_a_bar(x.position[0] / J_src.s.r), - y_a_bar(x.position[1] / J_src.s.r), - z_a_bar(x.position[2] / J_src.s.r), - delta_x(J_src.s.xi - x_a_bar), - delta_y(J_src.s.eta - y_a_bar), - delta_z(J_src.s.zeta - z_a_bar), - Delta3(std::pow( - std::pow(delta_x, 2) + std::pow(delta_y, 2) + std::pow(delta_z, 2), 1.5)); - J_x_as = mu_bar * (delta_x / Delta3 - J_src.s.xi); - J_y_as = mu_bar * (delta_y / Delta3 - J_src.s.eta); - J_z_as = mu_bar * (delta_z / Delta3 - J_src.s.zeta); - } - return differential_t(J_x_am + J_x_as, J_y_am + J_y_as, J_z_am + J_z_as)(t, x); + float_t J_am[3], J_as[3]; + calculate_Jm(x.position, J_am); + calculate_Js(x.position, J_as); + return differential_t(J_am[0] + J_as[0], J_am[1] + J_as[1], J_am[2] + J_as[2])(t, x); } }; diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp index c593c421d..aeb923fec 100644 --- a/tool/test/test_GLONASS.cpp +++ b/tool/test/test_GLONASS.cpp @@ -114,4 +114,37 @@ BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_J){ BOOST_REQUIRE_CLOSE(pos_vel.velocity[2], 1.040679862E3, 1E-1); } +BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_J_sun_moon){ + // TODO check precise method, J_m, J_s differences are not negligible. + + double t(11700); // [s] + + space_node_t::SatelliteProperties::Ephemeris::constellation_t pos_vel = { + {7523.174819E3, -10506.961965E3, 21999.239413E3}, + {0.950126007E3, 2.855687825E3, 1.040679862E3}, + }; + + space_node_t::SatelliteProperties::Ephemeris::constellation_t pos_vel_abs( + pos_vel.abs_corrdinate( + 29191.442830 + M_PI * 2 * (t - 10800) / 86400)); // PZ-90 => O-XYZ + + space_node_t::SatelliteProperties::Ephemeris::differential2_t diff2_2000 = { + space_node_t::SatelliteProperties::Ephemeris::lunar_solar_perturbations_t::base2000( + 2456177.5, t), + }; + + double Jm_2000[3], Js_2000[3]; + diff2_2000.calculate_Jm(pos_vel_abs.position, Jm_2000); + diff2_2000.calculate_Js(pos_vel_abs.position, Js_2000); + + space_node_t::SatelliteProperties::Ephemeris::differential2_t diff2_1975 = { + space_node_t::SatelliteProperties::Ephemeris::lunar_solar_perturbations_t::base1975( + 13704, t), // 13704 equals to interval days between 2012/7/9 and 1975/1/1 + }; + + double Jm_1975[3], Js_1975[3]; + diff2_1975.calculate_Jm(pos_vel_abs.position, Jm_1975); + diff2_1975.calculate_Js(pos_vel_abs.position, Js_1975); +} + BOOST_AUTO_TEST_SUITE_END() From f8f5dd4c1d40e006c83a39c9d59ef9ee1ce39906 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 28 Jan 2022 22:57:13 +0900 Subject: [PATCH 18/59] Move WGS84 conversion of PZ90.02 to constellation_t in GLONASS.h --- tool/navigation/GLONASS.h | 28 ++++++++++++++++------------ 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 6544d2b2b..7de519604 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -432,6 +432,20 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} }; return res; } + operator typename GPS_SpaceNode::SatelliteProperties + ::constellation_t() const { + // @see https://www.gsi.go.jp/common/000070971.pdf + // @see (originally) Federal Air Navigation Authority (FANA), Aeronautical Information Circular + // of the Russian Federation, 12 February 2009, Russia. + // PZ90.02 -> WGS84 + typename GPS_SpaceNode::SatelliteProperties::constellation_t res = { + typename GPS_SpaceNode::xyz_t( + position[0] - 0.36, position[1] + 0.08, position[2] + 0.18), + typename GPS_SpaceNode::xyz_t( + velocity[0], velocity[1], velocity[2]), + }; + return res; + } }; struct eccentric_anomaly_t { @@ -1013,7 +1027,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} using Ephemeris_with_Time::constellation; typename Ephemeris_with_Time::constellation_t constellation( const GPS_Time &t_arrival, const float_t &pseudo_range = 0) const { - return calculate_constellation(t_arrival - t_b_gps, pseudo_range); + return this->calculate_constellation(t_arrival - t_b_gps, pseudo_range); } }; }; @@ -1065,18 +1079,8 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} typename GPS_SpaceNode::SatelliteProperties::constellation_t constellation( const GPS_Time &t, const float_t &pseudo_range = 0) const { - typename eph_t::constellation_t constellation_PZ9002( + return (typename GPS_SpaceNode::SatelliteProperties::constellation_t)( ephemeris().constellation(t, pseudo_range)); - // @see https://www.gsi.go.jp/common/000070971.pdf - // @see (originally) Federal Air Navigation Authority (FANA), Aeronautical Information Circular - // of the Russian Federation, 12 February 2009, Russia. - float_t (&pos)[3](constellation_PZ9002.position); - float_t (&vel)[3](constellation_PZ9002.velocity); - typename GPS_SpaceNode::SatelliteProperties::constellation_t res = { - typename GPS_SpaceNode::xyz_t(pos[0] - 0.36, pos[1] + 0.08, pos[2] + 0.18), - typename GPS_SpaceNode::xyz_t(vel[0], vel[1], vel[2]), - }; - return res; } typename GPS_SpaceNode::xyz_t position(const GPS_Time &t, const float_t &pseudo_range = 0) const { From aec7b997d609cc94b2eb8c75cbd4528572e871b0 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sun, 30 Jan 2022 00:50:37 +0900 Subject: [PATCH 19/59] Add GLONASS interface to receiver_debug (not reflected to solution) --- tool/misc/receiver_debug.rb | 95 +++++++++++++++++++- tool/swig/GPS.i | 168 +++++++++++++++++++++++++++++++++++- 2 files changed, 258 insertions(+), 5 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 299b28197..989fb7f8e 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -368,7 +368,13 @@ def run(meas, t_meas, ref_pos = @base_station) eph.svid = prn [prn, eph] }.flatten(1)] - define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data| + eph_glonass_list = Hash[*(1..24).collect{|num| + eph = GPS::Ephemeris_GLONASS::new + eph.svid = num + [num, eph] + }.flatten(1)] + define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data, *options| + opt = options[0] || {} case sys when :GPS next unless eph = eph_list[prn] @@ -390,6 +396,15 @@ def run(meas, t_meas, ref_pos = @base_station) sn.register_ephemeris(prn, eph) eph.invalidate end + when :GLONASS + next unless eph = eph_glonass_list[prn] + leap_sec = @solver.gps_space_node.is_valid_utc ? + @solver.gps_space_node.iono_utc.delta_t_LS : + GPS::Time::guess_leap_seconds(t_meas) + next unless eph.parse(bcast_data[0..3], leap_sec) + eph.freq_ch = opt[:freq_ch] || 0 + @solver.glonass_space_node.register_ephemeris(prn, eph) + eph.invalidate end } }.call @@ -467,7 +482,9 @@ def parse_ubx(ubx_fname, &b) } sys, svid = gnss_serial.call(*loader.call(36, 2).reverse) case sys - when :GPS; + when :GPS; + when :GLONASS + svid += 0x100 else; next end trk_stat = loader.call(46, 1)[0] @@ -501,12 +518,14 @@ def parse_ubx(ubx_fname, &b) }) when [0x02, 0x13] # RXM-SFRBX sys, svid = gnss_serial.call(packet[6 + 1], packet[6]) + opt = {} + opt[:freq_ch] = packet[6 + 3] - 7 if sys == :GLONASS register_ephemeris( t_meas, sys, svid, packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v| v.pack("C*").unpack("V")[0] - }) + }, opt) end } $stderr.puts ", found packets are %s"%[ubx_kind.inspect] @@ -515,6 +534,7 @@ def parse_ubx(ubx_fname, &b) def parse_rinex_nav(fname) items = [ @solver.gps_space_node, + @solver.glonass_space_node, ].inject(0){|res, sn| loaded_items = sn.send(:read, fname) raise "Format error! (Not RINEX) #{fname}" if loaded_items < 0 @@ -555,6 +575,7 @@ def parse_rinex_obs(fname, &b) case sys when 'G', ' ' when 'J'; prn += 192 + when 'R'; prn += 0x100 else; next end types[sys] = (types[' '] || []) unless types[sys] @@ -641,7 +662,7 @@ def attach_antex(fname) files.collect!{|fname, ftype| raise "File not found: #{fname}" unless File::exist?(fname) ftype ||= case fname - when /\.\d{2}n$/; :rinex_nav + when /\.\d{2}[ng]$/; :rinex_nav when /\.\d{2}o$/; :rinex_obs when /\.ubx$/; :ubx when /\.sp3$/; :sp3 @@ -682,6 +703,57 @@ def attach_antex(fname) } }.call if [:start_time, :end_time].any?{|k| misc_options[k]} + rcv.solver.hooks[:satellite_position] = proc{|prn, time, pos| + next pos if pos + if prn & 0x100 == 0x100 then # GLONASS + eph = rcv.solver.glonass_space_node.ephemeris(prn - 0x100) + next nil if (eph.base_time - time).abs > 60 * 60 * 2 + next eph.constellation(time)[0] + end + nil + } + + glonass_residuals = {} # => {time => {svid => [residual, other_props]}, ...} + + proc{ + orig_hook = rcv.solver.hooks[:relative_property] + rcv.solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel| + rel_prop = orig_hook.call(prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel) if orig_hook + weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop # relative property + + while (prn & 0x100) == 0x100 # GLONASS + svid = prn - 0x100 + eph = rcv.solver.glonass_space_node.ephemeris(svid) + break if (eph.base_time - t_arv).abs >= 60 * 60 * 2 + break unless range = meas[GPS::Measurement::L1_PSEUDORANGE] + range -= rcv_e + clk_error = eph.clock_error(t_arv, range) * GPS::SpaceNode::light_speed + range += clk_error + sat_pos = eph.constellation(t_arv, range)[0] + rel_pos = sat_pos - usr_pos + rel_pos_enu = Coordinate::ENU::relative_rel(rel_pos, usr_pos) + range -= rel_pos.dist + sn = rcv.solver.gps_space_node + # GPS ICD 20.3.3.3.3.2 L1 - L2 Correction. + gamma = GPS::SpaceNode.gamma_per_L1(eph.frequency_L1) + iono = gamma * sn.iono_correction(rel_pos_enu, usr_pos.llh, t_arv) + tropo = GPS::SpaceNode::tropo_correction(sat_pos, usr_pos) + residual = range + iono + tropo + el, az = [:elevation, :azimuth].collect{|f| rel_pos_enu.send(f) / Math::PI * 180} + + t_arv = [t_arv.week, t_arv.seconds.round(1)] # week, ms + glonass_residuals[t_arv] ||= {} + if (!glonass_residuals[t_arv][svid]) \ + || (glonass_residuals[t_arv][svid][0].abs > residual.abs) then + glonass_residuals[t_arv][svid] = [residual, el, az, clk_error, iono, tropo] + end + break + end + + [weight, range_c, range_r, rate_rel_neg] + los_neg # must return relative property + } + }.call + puts rcv.header # parse RINEX NAV, SP3, or ANTEX @@ -700,4 +772,19 @@ def attach_antex(fname) when :rinex_obs; rcv.parse_rinex_obs(fname) end } + + open("glonass.csv", 'w'){|io| + io.puts(([:week, :seconds_in_week, + :year, :month, :day, :hour, :min, :seconds] + (1..24).collect{|svid| + [:residual, :el, :az, :clk_error, :iono, :tropo].collect{|k| + "#{k}(%d)"%[svid] + } + }).flatten.join(',')) + glonass_residuals.to_a.sort{|a, b| a[0] <=> b[0]}.each{|t, sv_props| + t = GPS::Time::new(*t[0..1]) + io.puts(([t.week, "%.3f"%[t.seconds]] + t.c_tm.to_a + (1..24).collect{|svid| + sv_props[svid] || ([nil] * 6) + }).flatten.join(',')) + } + } end diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index c4aca9103..23e1e4ce1 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -17,6 +17,7 @@ #include #include "navigation/GPS.h" +#include "navigation/GLONASS.h" #include "navigation/RINEX.h" #include "navigation/SP3.h" #include "navigation/ANTEX.h" @@ -434,6 +435,160 @@ struct GPS_Ephemeris : public GPS_SpaceNode::SatelliteProperties::Epheme %include navigation/GPS.h +%inline %{ +template +struct GLONASS_Ephemeris + : public GLONASS_SpaceNode::SatelliteProperties::Ephemeris_with_GPS_Time { + typedef typename GLONASS_SpaceNode::SatelliteProperties::Ephemeris_with_GPS_Time eph_t; + unsigned int super_frame, has_string; + typename eph_t::raw_t raw; + void invalidate() { + super_frame = 0; + has_string = 0; + } + bool is_consistent() const { + return has_string == 0x1F; + } + GLONASS_Ephemeris() : eph_t() { + invalidate(); + } + GLONASS_Ephemeris(const eph_t &eph) + : eph_t(eph), + super_frame(0), has_string(0), raw() { + raw = *this; + has_string = 0x1F; + } +}; +%} +%extend GLONASS_Ephemeris { + MAKE_ACCESSOR(svid, unsigned int); + + MAKE_ACCESSOR(freq_ch, int); // frequency channel to be configured + MAKE_ACCESSOR(t_k, unsigned int); + MAKE_ACCESSOR(t_b, unsigned int); + MAKE_ACCESSOR(M, unsigned int); + MAKE_ACCESSOR(gamma_n, FloatT); + MAKE_ACCESSOR(tau_n, FloatT); + + MAKE_ACCESSOR(xn, FloatT); MAKE_ACCESSOR(xn_dot, FloatT); MAKE_ACCESSOR(xn_ddot, FloatT); + MAKE_ACCESSOR(yn, FloatT); MAKE_ACCESSOR(yn_dot, FloatT); MAKE_ACCESSOR(yn_ddot, FloatT); + MAKE_ACCESSOR(zn, FloatT); MAKE_ACCESSOR(zn_dot, FloatT); MAKE_ACCESSOR(zn_ddot, FloatT); + + MAKE_ACCESSOR(B_n, unsigned int); + MAKE_ACCESSOR(p, unsigned int); + MAKE_ACCESSOR(N_T, unsigned int); + MAKE_ACCESSOR(F_T, FloatT); + MAKE_ACCESSOR(n, unsigned int); + MAKE_ACCESSOR(delta_tau_n, FloatT); + MAKE_ACCESSOR(E_n, unsigned int); + MAKE_ACCESSOR(P1, unsigned int); + MAKE_ACCESSOR(P2, bool); + MAKE_ACCESSOR(P4, bool); + + MAKE_ACCESSOR(tau_c, FloatT); + MAKE_ACCESSOR(tau_GPS, FloatT); + + FloatT frequency_L1() const { + return self->frequncy_L1(); + }; + FloatT frequency_L2() const { + return self->frequncy_L1(); + }; + GPS_Time base_time() const { + return self->base_time(); + } + + //MAKE_ACCESSOR(l_n, bool); // exists in both Ephemeris and Time_Properties + + MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int)); + bool parse(const unsigned int buf[4], const unsigned int &leap_seconds = 0){ + typedef typename GLONASS_SpaceNode + ::template BroadcastedMessage parser_t; + unsigned int super_frame(buf[3] >> 16), frame(buf[3] & 0xF), string_no(parser_t::m(buf)); + unsigned int has_string(self->has_string); + if((has_string > 0) && (self->super_frame != super_frame)){ + has_string = 0; // clean up + } + self->super_frame = super_frame; + has_string |= (0x1 << (string_no - 1)); + switch(string_no){ + case 1: self->raw.template update_string1<0, 0>(buf); break; + case 2: self->raw.template update_string2<0, 0>(buf); break; + case 3: self->raw.template update_string3<0, 0>(buf); break; + case 4: self->raw.template update_string4<0, 0>(buf); break; + case 5: { + self->raw.template update_string5<0, 0>(buf); + if(frame == 4){ + // TODO: require special care for 50th frame? @see Table 4.9 note (4) + } + break; + } + } + bool updated(false); + if((has_string == 0x1F) && (has_string != self->has_string)){ + updated = true; + // All ephemeris and time info. in the same super frame has been acquired, + // and this block is called once per one same super frame. + // Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time + static_cast::eph_t &>(*self) + = GLONASS_Ephemeris::eph_t(self->raw); + self->t_b_gps += leap_seconds; + } + self->has_string = has_string; + return updated; + } + FloatT clock_error( + const GPS_Time &t_arrival, const FloatT &pseudo_range = 0) const { + return self->clock_error(t_arrival, pseudo_range); + } + %typemap(in,numinputs=0) System_XYZ & (System_XYZ temp) %{ + $1 = &temp; + %} + %typemap(argout) System_XYZ & { + %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN)); + } + void constellation( + System_XYZ &position, System_XYZ &velocity, + const GPS_Time &t, const FloatT &pseudo_range = 0) const { + typename GPS_SpaceNode::SatelliteProperties::constellation_t res( + self->constellation(t, pseudo_range)); + position = res.position; + velocity = res.velocity; + } +#if defined(SWIGRUBY) + %rename("consistent?") is_consistent; + %rename("in_range?") is_in_range; +#endif + bool is_in_range(const GPS_Time &t) const { + // "invalidate()" is used to make raw and converted data inconsistent. + return self->is_valid(t); + } +} + +%extend GLONASS_SpaceNode { + %fragment(SWIG_Traits_frag(FloatT)); + %ignore satellites() const; + %ignore satellite(const int &); + %ignore latest_ephemeris() const; + void register_ephemeris( + const int &prn, const GLONASS_Ephemeris &eph, + const int &priority_delta = 1){ + self->satellite(prn).register_ephemeris(eph, priority_delta); + } + GLONASS_Ephemeris ephemeris(const int &prn) const { + return GLONASS_Ephemeris( + %const_cast(self, GLONASS_SpaceNode *)->satellite(prn).ephemeris()); + } + int read(const char *fname) { + std::fstream fin(fname, std::ios::in | std::ios::binary); + typename RINEX_NAV_Reader::space_node_list_t list = {NULL}; + list.glonass = self; + return RINEX_NAV_Reader::read_all(fin, list); + } +} + +%include navigation/GLONASS.h + %extend GPS_User_PVT { %ignore solver_t; %ignore base_t; @@ -832,6 +987,8 @@ struct GPS_RangeCorrector %ignore base_t; %ignore gps_t; %ignore gps; + %ignore glonass_t; + %ignore glonass; %ignore select_solver; %ignore relative_property; %ignore select_satellite; @@ -1104,6 +1261,10 @@ struct GPS_Solver GPS_SinglePositioning solver; gps_t() : space_node(), options(), solver(space_node) {} } gps; + struct glonass_t { + GLONASS_SpaceNode space_node; + glonass_t() : space_node() {} + } glonass; SWIG_Object hooks; typedef std::vector > user_correctors_t; user_correctors_t user_correctors; @@ -1119,7 +1280,7 @@ struct GPS_Solver } #endif GPS_Solver() : super_t(), - gps(), + gps(), glonass(), hooks(), user_correctors() { #ifdef SWIGRUBY hooks = rb_hash_new(); @@ -1127,6 +1288,7 @@ struct GPS_Solver } GPS_SpaceNode &gps_space_node() {return gps.space_node;} GPS_SolverOptions &gps_options() {return gps.options;} + GLONASS_SpaceNode &glonass_space_node() {return glonass.space_node;} const base_t &select_solver( const typename base_t::prn_t &prn) const { if(prn > 0 && prn <= 32){return gps.solver;} @@ -1154,6 +1316,7 @@ struct GPS_Solver const GPS_Time &receiver_time) const { const_cast(gps).space_node.update_all_ephemeris(receiver_time); const_cast(gps).solver.update_options(gps.options); + const_cast(glonass).space_node.update_all_ephemeris(receiver_time); return super_t::solve().user_pvt(measurement.items, receiver_time); } typedef @@ -1427,6 +1590,9 @@ struct SP3 : public SP3_Product { #endif %template(Solver) GPS_Solver; +%template(SpaceNode_GLONASS) GLONASS_SpaceNode; +%template(Ephemeris_GLONASS) GLONASS_Ephemeris; + %template(RINEX_Observation) RINEX_Observation; %template(SP3) SP3; %enddef From 1ca6847c1e59398af47ce2cb2130f0f5457c887f Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 2 Feb 2022 09:47:46 +0900 Subject: [PATCH 20/59] Enhance calculate_constellation of GLONASS Ephemeris to utilize cache --- tool/navigation/GLONASS.h | 28 +++++++++++++++++++++++++--- 1 file changed, 25 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 7de519604..c59f6a4e6 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -967,11 +967,23 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return calculate_clock_error( t_arrival_glonass + TimeProperties::tau_c - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale } + /** + * Calculate constellation(t) based on constellation(t_0). + * t_0 is a time around t_b, and is used to calculate + * an intermediate result specified with the 3rd argument. + * This method is useful to calculate constellation effectively + * by using a cache. + * @param delta_t time interval from t_0 to t + * @param pseudo_range measured pusedo_range to correct delta_t + * @param xa_t_0 constellation(t_0) + * @param t_0_from_t_b time interval from t_b to t_0 + */ constellation_t calculate_constellation( - float_t delta_t, const float_t &pseudo_range = 0) const { + float_t delta_t, const float_t &pseudo_range, + const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { delta_t -= pseudo_range / light_speed; - constellation_t res(xa_t_b); + constellation_t res(xa_t_0); { // time integration from t_b to t_arrival float_t t_step_max(delta_t >= 0 ? 60 : -60); int i(std::floor(delta_t / t_step_max)); @@ -984,7 +996,17 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } static const float_t omega_E(0.7292115E-4); // Earth's rotation rate, TODO move to PZ-90.02 - return res.rel_corrdinate(sidereal_t_b_rad + (omega_E * delta_t)); // transform from abs to PZ-90.02 + return res.rel_corrdinate( + sidereal_t_b_rad + (omega_E * (delta_t + t_0_from_t_b))); // transform from abs to PZ-90.02 + } + /** + * Calculate constellation(t) based on constellation(t_b). + * @param delta_t time interval from t_0 to t + * @param pseudo_range measured pusedo_range to correct delta_t, default is 0. + */ + constellation_t calculate_constellation( + float_t delta_t, const float_t &pseudo_range = 0) const { + return calculate_constellation(delta_t, pseudo_range, xa_t_b, float_t(0)); } /** * @param t_arrival_glonass signal arrival time in GLONASS time scale (t_GL = t_UTC + 3 hr + tau_c). From d4c07575eedef2291cb2de4862667fa4e37d9116 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Feb 2022 00:15:50 +0900 Subject: [PATCH 21/59] Add GLONASS solver (not activated due to exclusion of measurement) --- tool/INS_GPS/GNSS_Data.h | 15 +- tool/INS_GPS/GNSS_Receiver.h | 108 ++++++++++-- tool/navigation/GLONASS_Solver.h | 282 +++++++++++++++++++++++++++++++ 3 files changed, 385 insertions(+), 20 deletions(-) create mode 100644 tool/navigation/GLONASS_Solver.h diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index cf316fcca..96e340723 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -61,6 +61,8 @@ struct GNSS_Data { struct Loader { typedef GPS_SpaceNode gps_t; gps_t *gps; + typedef GLONASS_SpaceNode glonass_t; + glonass_t *glonass; typedef typename gps_t::Satellite::Ephemeris gps_ephemeris_t; struct gps_ephemeris_raw_t : public gps_ephemeris_t::raw_t { @@ -73,7 +75,6 @@ struct GNSS_Data { typedef typename gps_t::Ionospheric_UTC_Parameters gps_iono_utc_t; - typedef GLONASS_SpaceNode glonass_t; typedef typename glonass_t::Satellite::Ephemeris_with_GPS_Time glonass_ephemeris_t; struct glonass_ephemeris_raw_t : public glonass_ephemeris_t::raw_t { unsigned int super_frame; @@ -82,7 +83,7 @@ struct GNSS_Data { } glonass_ephemeris[24]; - Loader() : gps(NULL) { + Loader() : gps(NULL), glonass(NULL) { for(unsigned int i(0); i < sizeof(gps_ephemeris) / sizeof(gps_ephemeris[0]); ++i){ gps_ephemeris[i].svid = i + 1; } @@ -170,6 +171,12 @@ struct GNSS_Data { return false; } + bool load(const glonass_ephemeris_t &eph){ + if(!glonass){return false;} + glonass->satellite(eph.svid).register_ephemeris(eph); + return true; + } + bool load_glonass(const GNSS_Data &data){ if(data.subframe.bytes != 16){return false;} if((data.subframe.sv_number == 0) @@ -203,11 +210,11 @@ struct GNSS_Data { if(eph.has_string == 0x1F){ // get all ephemeris and time info. in the same super frame // Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time glonass_ephemeris_t eph_converted(eph); - eph_converted.freq_ch = data.subframe.glonass_freq_ch; // frequncy channel + eph_converted.freq_ch = data.subframe.glonass_freq_ch; // frequency channel eph_converted.t_b_gps += gps->is_valid_utc() // leap second correction ? gps->iono_utc().delta_t_LS : gps_time_t::guess_leap_seconds(eph_converted.t_b_gps); - // TODO register ephemeris + load(eph_converted); // register ephemeris eph.has_string = 0; } return true; diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 565bc723a..8c6c1cb71 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -37,7 +37,9 @@ #include #include "navigation/GPS.h" +#include "navigation/GLONASS.h" #include "navigation/GPS_Solver.h" +#include "navigation/GLONASS_Solver.h" #include "navigation/RINEX.h" #include "navigation/INS_GPS2_Tightly.h" @@ -84,6 +86,9 @@ struct GNSS_Receiver { typedef GPS_SinglePositioning gps_solver_t; #endif + typedef GLONASS_SpaceNode glonass_space_node_t; + typedef GLONASS_SinglePositioning glonass_solver_t; + struct system_t; struct data_t { @@ -91,15 +96,21 @@ struct GNSS_Receiver { gps_space_node_t space_node; typename gps_solver_t::options_t solver_options; } gps; + struct { + glonass_space_node_t space_node; + typename glonass_solver_t::options_t solver_options; + } glonass; std::ostream *out_rinex_nav; #if !defined(BUILD_WITHOUT_SP3) typedef SP3_Product sp3_t; sp3_t sp3; #endif - data_t() : gps(), out_rinex_nav(NULL) {} + data_t() : gps(), glonass(), out_rinex_nav(NULL) {} ~data_t(){ if(out_rinex_nav){ - RINEX_NAV_Writer::write_all(*out_rinex_nav, gps.space_node); + typename RINEX_NAV_Writer::space_node_list_t list = {&gps.space_node}; + list.glonass = &glonass.space_node; + RINEX_NAV_Writer::write_all(*out_rinex_nav, list); } } } data; @@ -107,18 +118,23 @@ struct GNSS_Receiver { struct solver_t : public solver_base_t { typedef solver_base_t base_t; gps_solver_t gps; + glonass_solver_t glonass; struct measurement_items_t : public gps_solver_t::measurement_items_t { // TODO }; solver_t(const GNSS_Receiver &rcv) : base_t(), - gps(rcv.data.gps.space_node) - {} + gps(rcv.data.gps.space_node), + glonass(rcv.data.glonass.space_node) { + glonass.ionospheric_correction.add(&gps.ionospheric_klobuchar); + glonass.tropospheric_correction.add(&gps.tropospheric_simplified); + } // Proxy functions const base_t &select(const typename base_t::prn_t &serial) const { switch(system_t::serial2system(serial)){ case system_t::GPS: return gps; + case system_t::GLONASS: return glonass; } return *this; } @@ -141,6 +157,8 @@ struct GNSS_Receiver { } root[] = { MAKE_ENTRY(gps.ionospheric_correction), MAKE_ENTRY(gps.tropospheric_correction), + MAKE_ENTRY(glonass.ionospheric_correction), + MAKE_ENTRY(glonass.tropospheric_correction), }; #undef MAKE_ENTRY for(std::size_t i(0); i < sizeof(root) / sizeof(root[0]); ++i){ @@ -180,6 +198,7 @@ struct GNSS_Receiver { void setup(typename GNSS_Data::Loader &loader) const { loader.gps = &const_cast(data.gps.space_node); + loader.glonass = &const_cast(data.glonass.space_node); } const solver_base_t &solver() const { @@ -193,10 +212,12 @@ struct GNSS_Receiver { void adjust(const GPS_Time &t){ // Select most preferable ephemeris data.gps.space_node.update_all_ephemeris(t); + data.glonass.space_node.update_all_ephemeris(t); // Solver update mainly for preferable ionospheric model selection // based on their availability solver_GNSS.gps.update_options(data.gps.solver_options); + solver_GNSS.glonass.update_options(data.glonass.solver_options); } struct system_t { @@ -336,8 +357,9 @@ struct GNSS_Receiver { if(dry_run){return true;} std::cerr << "RINEX Navigation file (" << value << ") reading..." << std::endl; std::istream &in(options.spec2istream(value)); - int ephemeris(RINEX_NAV_Reader::read_all( - in, data.gps.space_node)); + typename RINEX_NAV_Reader::space_node_list_t list = {&data.gps.space_node}; + list.glonass = &data.glonass.space_node; + int ephemeris(RINEX_NAV_Reader::read_all(in, list)); if(ephemeris < 0){ std::cerr << "(error!) Invalid format!" << std::endl; return false; @@ -389,7 +411,8 @@ struct GNSS_Receiver { #endif #define option_apply(expr) \ -data.gps.solver_options. expr +data.gps.solver_options. expr; \ +data.glonass.solver_options. expr if(value = runtime_opt_t::get_value(spec, "GNSS_elv_mask_deg", false)){ if(dry_run){return true;} FloatT mask_deg(std::atof(value)); @@ -416,6 +439,7 @@ data.gps.solver_options. expr std::cerr << "F10.7: " << f_10_7 << std::endl; solver_GNSS.gps.ionospheric_ntcm_gl.f_10_7 = f_10_7; solver_GNSS.gps.ionospheric_correction.add(&solver_GNSS.gps.ionospheric_ntcm_gl); + solver_GNSS.glonass.ionospheric_correction.add(&solver_GNSS.gps.ionospheric_ntcm_gl); return true; } @@ -465,6 +489,11 @@ data.gps.solver_options. expr ? data.gps.solver_options.exclude_prn.set(without) : data.gps.solver_options.exclude_prn.set(sv_id, without); break; + case system_t::GLONASS: + select_all + ? data.glonass.solver_options.exclude_prn.set(without) + : data.glonass.solver_options.exclude_prn.set(sv_id, without); + break; default: std::cerr << "(error!) Unsupported satellite! [" << value << "]" << std::endl; return false; @@ -511,7 +540,11 @@ data.gps.solver_options. expr << ',' << "v_down" << ',' << "receiver_clock_error_dot_ms" << ',' << "used_satellites" - << ',' << "PRN"; + << ',' << "GPS_PRN(1-32)" +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + << ',' << "GLONASS_PRN(1-24)" +#endif + ; } template static std::ostream &print(std::ostream &out, const GPS_PVT_Debug *){ @@ -520,6 +553,12 @@ data.gps.solver_options. expr out << ',' << "range_residual(" << i << ')' << ',' << "weight(" << i << ')'; } +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + for(int i(1); i <= 24; ++i){ + out << ',' << "range_residual(GLONASS:" << i << ')' + << ',' << "weight(GLONASS:" << i << ')'; + } +#endif return out; } #if !defined(BUILD_WITHOUT_GNSS_RAIM) @@ -545,6 +584,13 @@ data.gps.solver_options. expr const typename pvt_t::satellite_mask_t &_mask, const int &_prn_lsb, const int &_prn_msb) : mask(_mask), prn_lsb(_prn_lsb), prn_msb(_prn_msb) {} + mask_printer_t( + const typename pvt_t::satellite_mask_t &_mask, + const int &svid_lsb, const int &svid_msb, + const typename system_t::type_t &type) + : mask(_mask), + prn_lsb(satellite_id_t(type, svid_lsb)), + prn_msb(satellite_id_t(type, svid_msb)) {} friend std::ostream &operator<<(std::ostream &out, const mask_printer_t &p){ if(p.prn_msb < p.prn_lsb){return out;} int prn(p.prn_lsb % 8); @@ -590,9 +636,17 @@ data.gps.solver_options. expr } if(src.position_solved()){ out << ',' << src.used_satellites - << ',' << mask_printer_t(src.used_satellite_mask, 1, 32); + << ',' << mask_printer_t(src.used_satellite_mask, 1, 32) +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + << ',' << mask_printer_t(src.used_satellite_mask, 1, 24, system_t::GLONASS) // GLONASS +#endif + ; }else{ - out << ",,"; + out << ",," +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + << "," +#endif + ; } return out; } @@ -605,7 +659,10 @@ data.gps.solver_options. expr static const struct { int prn_first, prn_last; } range[] = { - {1, 32}, + {1, 32}, // GPS +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + {satellite_id_t(system_t::GLONASS, 1), satellite_id_t(system_t::GLONASS, 24)}, // GLONASS +#endif }; unsigned int i_row(0); for(std::size_t i(0); i < sizeof(range) / sizeof(range[0]); ++i){ @@ -663,14 +720,22 @@ data.gps.solver_options. expr friend std::ostream &operator<<(std::ostream &out, const label_t &label){ out << "clock_index"; for(int i(1); i <= 32; ++i){ - out << ',' << "L1_range(" << i << ')' + out << ',' << "L1_range(GPS:" << i << ')' #if !defined(BUILD_WITHOUT_GNSS_MULTI_FREQUENCY) - << ',' << "L2_range(" << i << ')' + << ',' << "L2_range(GPS:" << i << ')' #endif - << ',' << "L1_rate(" << i << ')' - << ',' << "azimuth(" << i << ')' - << ',' << "elevation(" << i << ')'; + << ',' << "L1_rate(GPS:" << i << ')' + << ',' << "azimuth(GPS:" << i << ')' + << ',' << "elevation(GPS:" << i << ')'; + } +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + for(int i(1); i <= 24; ++i){ + out << ',' << "L1_range(GLONASS:" << i << ')' + << ',' << "L1_rate(GLONASS:" << i << ')' + << ',' << "azimuth(GLONASS:" << i << ')' + << ',' << "elevation(GLONASS:" << i << ')'; } +#endif return out; } } label; @@ -757,6 +822,17 @@ data.gps.solver_options. expr for(int i(1); i <= 32; ++i){ out << ',' << p(i, cmd_gps) << ',' << p.az_el(i); } +#if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) + static const cmd_t cmd_glonass[] = { + {items_t::L1_PSEUDORANGE, true}, // range + {items_t::L1_RANGE_RATE, false}, // rate + {items_t::L1_DOPPLER, false, print_t::l1_doppler2rate_with_freq}, // fallback to using doppler + }; + for(int i(satellite_id_t(system_t::GLONASS, 1)), i_end(satellite_id_t(system_t::GLONASS, 24)); + i <= i_end; ++i){ + out << ',' << p(i, cmd_glonass) << ',' << p.az_el(i); + } +#endif return out; } }; diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h new file mode 100644 index 000000000..89aad5af7 --- /dev/null +++ b/tool/navigation/GLONASS_Solver.h @@ -0,0 +1,282 @@ +/** + * @file GLONASS solver + * + */ + +/* + * Copyright (c) 2022, M.Naruoka (fenrir) + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted provided that the following conditions are met: + * + * - Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * - Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * - Neither the name of the naruoka.org nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, + * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef __GLONASS_SOLVER_H__ +#define __GLONASS_SOLVER_H__ + +#include "GLONASS.h" +#include "GPS_Solver_Base.h" +#include "GPS_Solver.h" + +#include + +template +struct GLONASS_SinglePositioning_Options : public GPS_Solver_GeneralOptions { + typedef GPS_Solver_GeneralOptions super_t; + + typename GPS_Solver_Base::options_t::template exclude_prn_t<1, 24> exclude_prn; // GLONASS svid ranges from 1 to 24 + GLONASS_SinglePositioning_Options() + : super_t(), exclude_prn() { + exclude_prn.set(true); // GLONASS ranging is default off. + } +}; + +template > +class GLONASS_SinglePositioning : public SolverBaseT { + private: + GLONASS_SinglePositioning &operator=(const GLONASS_SinglePositioning &); + public: + typedef GLONASS_SinglePositioning self_t; + typedef SolverBaseT base_t; + +#if defined(__GNUC__) && (__GNUC__ < 5) +#define inheritate_type(x) typedef typename base_t::x x; +#else +#define inheritate_type(x) using typename base_t::x; +#endif + + inheritate_type(float_t); + inheritate_type(prn_t); + + typedef GLONASS_SpaceNode space_node_t; + inheritate_type(gps_time_t); + + inheritate_type(xyz_t); + inheritate_type(enu_t); + + inheritate_type(pos_t); + + typedef typename base_t::measurement_t measurement_t; + typedef typename base_t::satellite_t satellite_t; + typedef typename base_t::range_error_t range_error_t; + typedef typename base_t::range_corrector_t range_corrector_t; + typedef typename base_t::range_correction_t range_correction_t; + + inheritate_type(relative_property_t); +#undef inheritate_type + + typedef typename GPS_Solver_Base::options_t::template merge_t< + GLONASS_SinglePositioning_Options, base_t> options_t; + + protected: + GLONASS_SinglePositioning_Options _options; + + public: + struct satellites_t { + const void *impl; + satellite_t (*impl_select)(const void *, const prn_t &, const gps_time_t &); + inline satellite_t select(const prn_t &prn, const gps_time_t &receiver_time) const { + return impl_select(impl, prn, receiver_time); + } + static satellite_t select_broadcast( + const void *ptr, const prn_t &prn, const gps_time_t &receiver_time){ + const typename space_node_t::satellites_t &sats( + reinterpret_cast(ptr)->satellites()); + const typename space_node_t::satellites_t::const_iterator it_sat(sats.find(prn)); + if((it_sat == sats.end()) // has ephemeris? + || (!it_sat->second.ephemeris().is_valid(receiver_time))){ // valid ephemeris? + return satellite_t::unavailable(); + } + struct impl_t { + static inline const typename space_node_t::Satellite &sat(const void *ptr) { + return *reinterpret_cast(ptr); + } + static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { + return sat(ptr).constellation(t, pseudo_range).position; + } + static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { + return sat(ptr).constellation(t, pseudo_range).velocity; + } + static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { + return sat(ptr).clock_error(t, pseudo_range); + } + static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { + // Clock rate error is already taken into account in constellation() + return 0; + } + }; + satellite_t res = { + &(it_sat->second), + impl_t::position, impl_t::velocity, + impl_t::clock_error, impl_t::clock_error_dot}; + return res; + } + satellites_t(const space_node_t &sn) + : impl(&sn), impl_select(select_broadcast) {} + } satellites; + + range_correction_t ionospheric_correction, tropospheric_correction; + + options_t available_options() const { + return options_t(base_t::available_options(), _options); + } + + options_t available_options(const options_t &opt_wish) const { + GLONASS_SinglePositioning_Options opt(opt_wish); + return options_t(base_t::available_options(opt_wish), opt); + } + + options_t update_options(const options_t &opt_wish){ + _options = opt_wish; + return options_t(base_t::update_options(opt_wish), _options); + } + + GLONASS_SinglePositioning(const space_node_t &sn, const options_t &opt_wish = options_t()) + : base_t(), _options(available_options(opt_wish)), + satellites(sn), + ionospheric_correction(), tropospheric_correction() { + + // default ionospheric correction: no correction + ionospheric_correction.push_front(&base_t::no_correction); + + // default troposheric correction: no correction + tropospheric_correction.push_front(&base_t::no_correction); + } + + ~GLONASS_SinglePositioning(){} + + /** + * Select satellite by using PRN and time + * + * @param prn satellite number + * @param receiver_time receiver time + * @return (satellite_t) If available, satellite.is_available() returning true is returned. + */ + satellite_t select_satellite( + const prn_t &prn, + const gps_time_t &receiver_time) const { + prn_t prn_(prn & 0xFF); + if(_options.exclude_prn[prn_]){return satellite_t::unavailable();} + return satellites.select(prn_, receiver_time); + } + + /** + * Calculate relative range and rate information to a satellite + * + * @param prn satellite number + * @param measurement measurement (per satellite) containing pseudo range + * @param receiver_error (temporal solution of) receiver clock error in meter + * @param time_arrival time when signal arrive at receiver + * @param usr_pos (temporal solution of) user position + * @param usr_vel (temporal solution of) user velocity + * @return (relative_property_t) relative information + */ + relative_property_t relative_property( + const prn_t &prn, + const typename measurement_t::mapped_type &measurement, + const float_t &receiver_error, + const gps_time_t &time_arrival, + const pos_t &usr_pos, + const xyz_t &usr_vel) const { + + relative_property_t res = {0}; + + float_t range; + range_error_t range_error; + if(!this->range(measurement, range, &range_error)){ + return res; // If no range entry, return with weight = 0 + } + + satellite_t sat(select_satellite(prn, time_arrival)); + if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0 + + ///< The following procedure is based on Appendix.S with modification + + range -= receiver_error; + + // Clock correction will be performed in the following constellation() + if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){ + range += (sat.clock_error(time_arrival, range) * space_node_t::light_speed); + }else{ + range += range_error.value[range_error_t::SATELLITE_CLOCK]; + } + + // Calculate satellite position + xyz_t sat_pos(sat.position(time_arrival, range)); + float_t geometric_range(usr_pos.xyz.dist(sat_pos)); + + // Calculate residual + res.range_residual = range - geometric_range; + + // Setup design matrix + res.los_neg[0] = -(sat_pos.x() - usr_pos.xyz.x()) / geometric_range; + res.los_neg[1] = -(sat_pos.y() - usr_pos.xyz.y()) / geometric_range; + res.los_neg[2] = -(sat_pos.z() - usr_pos.xyz.z()) / geometric_range; + + enu_t relative_pos(enu_t::relative(sat_pos, usr_pos.xyz)); + + // Tropospheric + res.range_residual += (range_error.unknown_flag & range_error_t::MASK_TROPOSPHERIC) + ? tropospheric_correction(time_arrival, usr_pos, relative_pos) + : range_error.value[range_error_t::TROPOSPHERIC]; + + // Ionospheric + if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){ + res.range_residual += ionospheric_correction(time_arrival, usr_pos, relative_pos); + }else{ + res.range_residual += range_error.value[range_error_t::IONOSPHERIC]; + } + + // Setup weight + if(std::abs(res.range_residual) > _options.residual_mask){ + // If residual is too big, gently exclude it by decreasing its weight. + res.weight = 1E-8; + }else{ + + float_t elv(relative_pos.elevation()); + if(elv < _options.elevation_mask){ + res.weight = 0; // exclude it when elevation is less than threshold + }else{ + // elevation weight based on "GPS実用プログラミング" @see GPS_Solver.h + res.weight = std::pow(sin(elv)/0.8, 2); + if(res.weight < 1E-3){res.weight = 1E-3;} + } + } + + res.range_corrected = range; + + xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity + + res.rate_relative_neg = res.los_neg[0] * rel_vel.x() + + res.los_neg[1] * rel_vel.y() + + res.los_neg[2] * rel_vel.z() + + sat.clock_error_dot(time_arrival, range) * space_node_t::light_speed; + + return res; + } +}; + +#endif /* __GLONASS_SOLVER_H__ */ From 9a42911ee558fe2102716774d1e6c79366a1a2e8 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Feb 2022 09:30:00 +0900 Subject: [PATCH 22/59] Move L1/L2_frequency() form GLONASS ephemerise to space node --- tool/navigation/GLONASS.h | 23 +++++++++++++++-------- tool/swig/GPS.i | 4 ++-- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index c59f6a4e6..0557e9848 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -53,9 +53,9 @@ class GLONASS_SpaceNode { typedef FloatT float_t; static const float_t light_speed; - static const float_t L1_frequency; + static const float_t L1_frequency_base; static const float_t L1_frequency_gap; - static const float_t L2_frequency; + static const float_t L2_frequency_base; static const float_t L2_frequency_gap; public: @@ -71,6 +71,13 @@ class GLONASS_SpaceNode { typedef int int_t; typedef unsigned int uint_t; + static float_t L1_frequency(const int_t &freq_ch) { + return L1_frequency_base + L1_frequency_gap * freq_ch; + } + static float_t L2_frequency(const int_t &freq_ch) { + return L2_frequency_base + L2_frequency_gap * freq_ch; + } + typedef typename GPS_SpaceNode::DataParser DataParser; template raw_t::sf[raw_t::SF_ ## TARGET]){break;} bool P4; // flag for ephemeris; 1 - uploaded by the control segment bool l_n; // health flag; 0 - healthy, 1 - malfunction - float_t frequncy_L1() const { - return L1_frequency + L1_frequency_gap * freq_ch; + float_t L1_frequency() const { + return GLONASS_SpaceNode::L1_frequency(freq_ch); } - float_t frequncy_L2() const { - return L2_frequency + L2_frequency_gap * freq_ch; + float_t L2_frequency() const { + return GLONASS_SpaceNode::L2_frequency(freq_ch); } struct constellation_t { // TODO make it to be a subclass of System_XYZ @@ -1159,7 +1166,7 @@ const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::lig = 299792458; // [m/s] template -const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_frequency +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_frequency_base = 1602E6; // [Hz] template @@ -1167,7 +1174,7 @@ const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_ = 562.5E3; // [Hz] template -const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L2_frequency +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L2_frequency_base = 1246E6; // [Hz] template diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 23e1e4ce1..fd2b7c409 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -489,10 +489,10 @@ struct GLONASS_Ephemeris MAKE_ACCESSOR(tau_GPS, FloatT); FloatT frequency_L1() const { - return self->frequncy_L1(); + return self->L1_frequency(); }; FloatT frequency_L2() const { - return self->frequncy_L1(); + return self->L2_frequency(); }; GPS_Time base_time() const { return self->base_time(); From 29a4a6edd419249a160c08a5cf083028656f066f Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Feb 2022 09:42:28 +0900 Subject: [PATCH 23/59] Activate GLONASS solution --- tool/INS_GPS.cpp | 8 +++++++ tool/INS_GPS/GNSS_Receiver.h | 2 ++ tool/navigation/GLONASS_Solver.h | 39 ++++++++++++++++++++++++++++++++ 3 files changed, 49 insertions(+) diff --git a/tool/INS_GPS.cpp b/tool/INS_GPS.cpp index fe78b4053..4046a810c 100644 --- a/tool/INS_GPS.cpp +++ b/tool/INS_GPS.cpp @@ -205,6 +205,7 @@ struct QuaternionData_TypeMapper { #include "navigation/INS_GPS_Synchronization.h" #include "navigation/INS_GPS_Debug.h" #include "navigation/GPS.h" +#include "navigation/GLONASS.h" #include "navigation/WGS84.h" #include "navigation/MagneticField.h" @@ -2032,6 +2033,13 @@ class StreamProcessor dst.insert(std::make_pair(signal->signal_strength, observer[6 + 42 + (32 * i)])); observer.inspect(buf, 2, 6 + 40 + (32 * i)); dst.insert(std::make_pair(signal->lock_sec, 1E-3 * le_char2_2_num(*buf))); + + if(gnssID == G_Observer_t::gnss_svid_t::GLONASS){ + int freq_ch(-7 + (int)observer[6 + 39 + (32 * i)]); + dst.insert(std::make_pair( + items_t::L1_FREQUENCY, + GLONASS_SpaceNode::L1_frequency(freq_ch))); + } } packet_raw_latest.update_measurement(current, measurement); diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 8c6c1cb71..83cf01c59 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -341,6 +341,8 @@ struct GNSS_Receiver { return &(gps_solver_t::L2CM); case decorder_t::gnss_signal_t::GPS_L2CL: return &(gps_solver_t::L2CL); + case decorder_t::gnss_signal_t::GLONASS_L1_OF: + return &(glonass_solver_t::L1OF); #endif } return NULL; // TODO support other GNSS, signals diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 89aad5af7..6e27c921e 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -61,6 +61,7 @@ class GLONASS_SinglePositioning : public SolverBaseT { public: typedef GLONASS_SinglePositioning self_t; typedef SolverBaseT base_t; + typedef SolverBaseT super_t; #if defined(__GNUC__) && (__GNUC__ < 5) #define inheritate_type(x) typedef typename base_t::x x; @@ -86,8 +87,11 @@ class GLONASS_SinglePositioning : public SolverBaseT { typedef typename base_t::range_correction_t range_correction_t; inheritate_type(relative_property_t); + typedef typename super_t::measurement_items_t measurement_items_t; #undef inheritate_type + static const typename base_t::measurement_item_set_t L1OF; + typedef typename GPS_Solver_Base::options_t::template merge_t< GLONASS_SinglePositioning_Options, base_t> options_t; @@ -168,6 +172,36 @@ class GLONASS_SinglePositioning : public SolverBaseT { ~GLONASS_SinglePositioning(){} + virtual const float_t *rate( + const typename measurement_t::mapped_type &values, float_t &buf) const { + const float_t *res; + if((res = super_t::find_value(values, measurement_items_t::L1_RANGE_RATE, buf))){ + return res; + } + // Fall back to doppler * frequency + float_t doppler, freq; + if(super_t::find_value(values, measurement_items_t::L1_DOPPLER, doppler) + && super_t::find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + return &(buf = -doppler * (space_node_t::light_speed / freq)); + } + return NULL; + } + + virtual const float_t *rate_sigma( + const typename measurement_t::mapped_type &values, float_t &buf) const { + const float_t *res; + if((res = super_t::find_value(values, measurement_items_t::L1_RANGE_RATE_SIGMA, buf))){ + return res; + } + // Fall back to doppler * frequency + float_t doppler, freq; + if(super_t::find_value(values, measurement_items_t::L1_DOPPLER_SIGMA, doppler) + && super_t::find_value(values, measurement_items_t::L1_FREQUENCY, freq)){ + return &(buf = doppler * (space_node_t::light_speed / freq)); + } + return NULL; + } + /** * Select satellite by using PRN and time * @@ -279,4 +313,9 @@ class GLONASS_SinglePositioning : public SolverBaseT { } }; +template +const typename SolverBaseT::measurement_item_set_t + GLONASS_SinglePositioning::L1OF + = SolverBaseT::L1CA; + #endif /* __GLONASS_SOLVER_H__ */ From b5d364cc0feacb0ea6722742550cfb871ddc78f1 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Feb 2022 23:56:37 +0900 Subject: [PATCH 24/59] Update Ruby receiver to use built-in GLONASS solver --- tool/misc/receiver_debug.rb | 50 ++++++++++++++++++++++++------------- tool/swig/GPS.i | 32 +++++++++++++++++++++++- 2 files changed, 64 insertions(+), 18 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 989fb7f8e..5eb54fc89 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -159,7 +159,7 @@ def initialize(options = {}) rel_prop } @debug = {} - solver_opts = [:gps_options].collect{|target| + solver_opts = [:gps_options, :glonass_options].collect{|target| @solver.send(target) } solver_opts.each{|opt| @@ -268,6 +268,12 @@ def initialize(options = {}) } if check_sys_svid.call(:GPS, 1..32) then [svid || (1..32).to_a].flatten.each{|prn| @solver.gps_options.send(mode, prn)} + elsif check_sys_svid.call(:GLONASS, 1..24, 0x100) then + prns = [svid || (1..24).to_a].flatten.collect{|i| (i & 0xFF) + 0x100} + labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"} + update_output.call(:GLONASS, prns, labels) + prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)} + #$stderr.puts @solver.glonass_options.excluded.inspect else raise "Unknown satellite: #{spec}" end @@ -485,6 +491,8 @@ def parse_ubx(ubx_fname, &b) when :GPS; when :GLONASS svid += 0x100 + meas.add(svid, :L1_FREQUENCY, + GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7)) else; next end trk_stat = loader.call(46, 1)[0] @@ -547,6 +555,7 @@ def parse_rinex_obs(fname, &b) after_run = b || proc{|pvt| puts pvt.to_s if pvt} $stderr.print "Reading RINEX observation file (%s)"%[fname] types = nil + glonass_freq = nil count = 0 GPS::RINEX_Observation::read(fname){|item| $stderr.print '.' if (count += 1) % 1000 == 0 @@ -569,13 +578,31 @@ def parse_rinex_obs(fname, &b) }.compact] }.flatten(1))] + glonass_freq ||= proc{|spec| + # frequency channels described in observation file + next {} unless spec + Hash[*(spec.collect{|line| + line[4..-1].scan(/R(\d{2}).([\s+-]\d)./).collect{|prn, ch| + [prn.to_i, GPS::SpaceNode_GLONASS::L1_frequency(ch.to_i)] + } + }.flatten(2))] + }.call(item[:header]["GLONASS SLOT / FRQ #"]) + meas = GPS::Measurement::new item[:meas].each{|k, v| sys, prn = k case sys when 'G', ' ' when 'J'; prn += 192 - when 'R'; prn += 0x100 + when 'R' + freq = (glonass_freq[prn] ||= proc{|sn| + # frequency channels saved with ephemeris + sn.update_all_ephemeris(t_meas) + next nil unless sn.ephemeris(prn).in_range?(t_meas) + sn.ephemeris(prn).frequency_L1 + }.call(@solver.glonass_space_node)) + prn += 0x100 + meas.add(prn, :L1_FREQUENCY, freq) if freq else; next end types[sys] = (types[' '] || []) unless types[sys] @@ -703,18 +730,7 @@ def attach_antex(fname) } }.call if [:start_time, :end_time].any?{|k| misc_options[k]} - rcv.solver.hooks[:satellite_position] = proc{|prn, time, pos| - next pos if pos - if prn & 0x100 == 0x100 then # GLONASS - eph = rcv.solver.glonass_space_node.ephemeris(prn - 0x100) - next nil if (eph.base_time - time).abs > 60 * 60 * 2 - next eph.constellation(time)[0] - end - nil - } - glonass_residuals = {} # => {time => {svid => [residual, other_props]}, ...} - proc{ orig_hook = rcv.solver.hooks[:relative_property] rcv.solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel| @@ -744,8 +760,8 @@ def attach_antex(fname) t_arv = [t_arv.week, t_arv.seconds.round(1)] # week, ms glonass_residuals[t_arv] ||= {} if (!glonass_residuals[t_arv][svid]) \ - || (glonass_residuals[t_arv][svid][0].abs > residual.abs) then - glonass_residuals[t_arv][svid] = [residual, el, az, clk_error, iono, tropo] + || (glonass_residuals[t_arv][svid][2].abs > residual.abs) then + glonass_residuals[t_arv][svid] = [weight, range_r, residual, el, az, clk_error, iono, tropo] end break end @@ -776,14 +792,14 @@ def attach_antex(fname) open("glonass.csv", 'w'){|io| io.puts(([:week, :seconds_in_week, :year, :month, :day, :hour, :min, :seconds] + (1..24).collect{|svid| - [:residual, :el, :az, :clk_error, :iono, :tropo].collect{|k| + [:weight_orig, :residual_orig, :residual, :el, :az, :clk_error, :iono, :tropo].collect{|k| "#{k}(%d)"%[svid] } }).flatten.join(',')) glonass_residuals.to_a.sort{|a, b| a[0] <=> b[0]}.each{|t, sv_props| t = GPS::Time::new(*t[0..1]) io.puts(([t.week, "%.3f"%[t.seconds]] + t.c_tm.to_a + (1..24).collect{|svid| - sv_props[svid] || ([nil] * 6) + sv_props[svid] || ([nil] * 8) }).flatten.join(',')) } } diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index fd2b7c409..4886b1d03 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -25,6 +25,7 @@ #include "navigation/GPS_Solver_Base.h" #include "navigation/GPS_Solver.h" #include "navigation/GPS_Solver_RAIM.h" +#include "navigation/GLONASS_Solver.h" #if defined(__cplusplus) && (__cplusplus < 201103L) #include @@ -961,6 +962,25 @@ struct GPS_SolverOptions }; %} +%extend GLONASS_SolverOptions { + %ignore base_t; + %ignore cast_general; + MAKE_VECTOR2ARRAY(int); +} +%inline %{ +template +struct GLONASS_SolverOptions + : public GLONASS_SinglePositioning::options_t, + GPS_SolverOptions_Common { + typedef typename GLONASS_SinglePositioning::options_t base_t; + void exclude(const int &prn){base_t::exclude_prn.set(prn);} + void include(const int &prn){base_t::exclude_prn.reset(prn);} + std::vector excluded() const {return base_t::exclude_prn.excluded();} + GPS_Solver_GeneralOptions *cast_general(){return this;} + const GPS_Solver_GeneralOptions *cast_general() const {return this;} +}; +%} + %header { template struct GPS_RangeCorrector @@ -1148,6 +1168,8 @@ struct GPS_RangeCorrector static const VALUE k_root[] = { ID2SYM(rb_intern("gps_ionospheric")), ID2SYM(rb_intern("gps_tropospheric")), + ID2SYM(rb_intern("glonass_ionospheric")), + ID2SYM(rb_intern("glonass_tropospheric")), }; static const VALUE k_opt(ID2SYM(rb_intern("options"))); static const VALUE k_f_10_7(ID2SYM(rb_intern("f_10_7"))); @@ -1263,7 +1285,9 @@ struct GPS_Solver } gps; struct glonass_t { GLONASS_SpaceNode space_node; - glonass_t() : space_node() {} + GLONASS_SolverOptions options; + GLONASS_SinglePositioning solver; + glonass_t() : space_node(), options(), solver(space_node) {} } glonass; SWIG_Object hooks; typedef std::vector > user_correctors_t; @@ -1289,9 +1313,11 @@ struct GPS_Solver GPS_SpaceNode &gps_space_node() {return gps.space_node;} GPS_SolverOptions &gps_options() {return gps.options;} GLONASS_SpaceNode &glonass_space_node() {return glonass.space_node;} + GLONASS_SolverOptions &glonass_options() {return glonass.options;} const base_t &select_solver( const typename base_t::prn_t &prn) const { if(prn > 0 && prn <= 32){return gps.solver;} + if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;} // call order: base_t::solve => this returned by select() // => relative_property() => select_solver() // For not supported satellite, call loop prevention is required. @@ -1317,6 +1343,7 @@ struct GPS_Solver const_cast(gps).space_node.update_all_ephemeris(receiver_time); const_cast(gps).solver.update_options(gps.options); const_cast(glonass).space_node.update_all_ephemeris(receiver_time); + const_cast(glonass).solver.update_options(glonass.options); return super_t::solve().user_pvt(measurement.items, receiver_time); } typedef @@ -1329,6 +1356,8 @@ struct GPS_Solver typename base_t::range_correction_t *root[] = { &gps.solver.ionospheric_correction, &gps.solver.tropospheric_correction, + &glonass.solver.ionospheric_correction, + &glonass.solver.tropospheric_correction, }; for(std::size_t i(0); i < sizeof(root) / sizeof(root[0]); ++i){ do{ @@ -1592,6 +1621,7 @@ struct SP3 : public SP3_Product { %template(SpaceNode_GLONASS) GLONASS_SpaceNode; %template(Ephemeris_GLONASS) GLONASS_Ephemeris; +%template(SolverOptions_GLONASS) GLONASS_SolverOptions; %template(RINEX_Observation) RINEX_Observation; %template(SP3) SP3; From a9ba176f401024dc32557c459948df833a6ad0ac Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 22 Feb 2022 10:18:37 +0900 Subject: [PATCH 25/59] Change ionospheric correction to consider frequency difference --- tool/navigation/GLONASS_Solver.h | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 6e27c921e..7dd797d69 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -38,6 +38,7 @@ #define __GLONASS_SOLVER_H__ #include "GLONASS.h" +#include "GPS.h" #include "GPS_Solver_Base.h" #include "GPS_Solver.h" @@ -279,7 +280,12 @@ class GLONASS_SinglePositioning : public SolverBaseT { // Ionospheric if(range_error.unknown_flag & range_error_t::MASK_IONOSPHERIC){ - res.range_residual += ionospheric_correction(time_arrival, usr_pos, relative_pos); + float_t freq(space_node_t::L1_frequency_base); // ch.0 is fallback + super_t::find_value(measurement, measurement_items_t::L1_FREQUENCY, freq); + // ionospheric models are assumed to work with GPS L1 + res.range_residual += + (ionospheric_correction(time_arrival, usr_pos, relative_pos) + * GPS_SpaceNode::gamma_per_L1(freq)); }else{ res.range_residual += range_error.value[range_error_t::IONOSPHERIC]; } From 743c6e94ae0188a4839ab522758a6dbe6af8f8a7 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 23 Feb 2022 21:44:27 +0900 Subject: [PATCH 26/59] Add calculation of difference between GLONASS and GPS time --- tool/misc/receiver_debug.rb | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 5eb54fc89..d8c858a13 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -31,11 +31,12 @@ def self.pvt_items(opt = {}) [:week, :seconds, :utc].collect{|f| pvt.receiver_time.send(f)}.flatten } ]] + [[ - [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U], + [:receiver_clock_error_meter, :delta_system, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U], proc{|pvt| - next [nil] * 7 unless pvt.position_solved? + next [nil] * 8 unless pvt.position_solved? [ pvt.receiver_error, + pvt.other_state[0], pvt.llh.lng / Math::PI * 180, pvt.llh.lat / Math::PI * 180, pvt.llh.alt, @@ -730,6 +731,17 @@ def attach_antex(fname) } }.call if [:start_time, :end_time].any?{|k| misc_options[k]} + rcv.solver.hooks[:update_position_solution] = proc{|mat_G, mat_W, mat_r, tmp_pvt| + i_glonass = [] + i_other = [] + tmp_pvt.used_satellite_list.each.with_index{|prn, i| + ((prn & 0x100 == 0x100) ? i_glonass : i_other) << i + } + next if (i_glonass.empty? || i_other.empty?) + mat_G.resize!(nil, 5) + i_glonass.each{|i| mat_G[i, 4] = 1} + } + glonass_residuals = {} # => {time => {svid => [residual, other_props]}, ...} proc{ orig_hook = rcv.solver.hooks[:relative_property] From 59b8149d755a2bf495146147910ec0ab4b54abbe Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 24 Feb 2022 21:28:33 +0900 Subject: [PATCH 27/59] Fix GLONASS satellite position calculation * Subtraction of transmission time was wrongly performed. --- tool/navigation/GLONASS.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 0557e9848..9133a7772 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -988,13 +988,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} constellation_t calculate_constellation( float_t delta_t, const float_t &pseudo_range, const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { - delta_t -= pseudo_range / light_speed; constellation_t res(xa_t_0); - { // time integration from t_b to t_arrival - float_t t_step_max(delta_t >= 0 ? 60 : -60); - int i(std::floor(delta_t / t_step_max)); - float_t t_step_remain(delta_t - t_step_max * i); + { // time integration from t_b to t_transmit + float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed); + float_t t_step_max(delta_t_to_transmit >= 0 ? 60 : -60); + int i(std::floor(delta_t_to_transmit / t_step_max)); + float_t t_step_remain(delta_t_to_transmit - t_step_max * i); float_t delta_t_itg(0); // accumulative time of integration for(; i > 0; --i, delta_t_itg += t_step_max){ res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_max); From b63578005344d0cfecb646418580cf55bd77fd07 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 25 Feb 2022 13:42:16 +0900 Subject: [PATCH 28/59] Add GLONASS extension in RINEX version 3.05 --- tool/navigation/GLONASS.h | 42 +++++++++++++----- tool/navigation/RINEX.h | 48 +++++++++++++++++++-- tool/test/test_RINEX.cpp | 89 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 164 insertions(+), 15 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 9133a7772..ab13cd6b0 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -694,6 +694,18 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} u8_t F_T_index() const; + u8_t P1_index() const { + if(P1 > 45 * 60){ + return 3; + }else if(P1 > 30 * 60){ + return 2; + }else if(P1 > 0){ + return 1; + }else{ + return 0; + } + } + struct raw_t { u8_t svid; @@ -789,6 +801,23 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} static const float_t F_T_table[15]; ///< @see Table 4.4 + static const float_t F_T_value(const u8_t &F_T_){ + if(F_T_ >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){ + return -1; // not used + }else{ + return F_T_table[F_T_]; + } + } + + static const uint_t P1_value(const u8_t &P1_){ + switch(P1_){ + case 1: return 30 * 60; + case 2: return 45 * 60; + case 3: return 60 * 60; + case 0: default: return 0; + } + } + operator Ephemeris() const { Ephemeris res; #define CONVERT(TARGET) \ @@ -808,20 +837,11 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} res.p = p; res.N_T = N_T; - if(F_T >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){ - res.F_T = -1; // not used - }else{ - res.F_T = F_T_table[F_T]; - } + res.F_T = F_T_value(F_T); res.n = n; CONVERT(delta_tau_n); res.E_n = E_n; - switch(P1){ - case 1: res.P1 = 30 * 60; break; - case 2: res.P1 = 45 * 60; break; - case 3: res.P1 = 60 * 60; break; - case 0: default: res.P1 = 0; break; - } + res.P1 = P1_value(P1); res.P2 = (P2 > 0); //res.P3 = (P3 > 0); res.P4 = (P4 > 0); diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 0fd20223b..46f9594e1 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -390,7 +390,19 @@ struct RINEX_NAV { FloatT z_km, dz_km_s, ddz_km_s2; unsigned int B_n, E_n; int freq_num; // 1-24(ver.2), -7-13(ver.3) - message_glonass_t() {} + + // since ver.3.05 + unsigned int status_flags, urai, health_flags; + FloatT delta_tau; + + message_glonass_t(){ + status_flags + = ((0x01 & 0x3) << 7) // GLONASS-M + | ((0x3) << 2); // upload/validity interval = 60 min + urai = 15; // unknown + health_flags = 0; + delta_tau = 0; + } message_glonass_t(const eph_t &eph) : svid((int)eph.svid), date_tm(eph.c_tm_utc()), @@ -403,7 +415,14 @@ struct RINEX_NAV { y_km(1E-3 * eph.yn), dy_km_s(1E-3 * eph.yn_dot), ddy_km_s2(1E-3 * eph.yn_ddot), z_km(1E-3 * eph.zn), dz_km_s(1E-3 * eph.zn_dot), ddz_km_s2(1E-3 * eph.zn_ddot), B_n(eph.B_n), E_n(eph.E_n), - freq_num(eph.freq_ch) { + freq_num(eph.freq_ch), + urai(eph.F_T_index()), delta_tau(eph.delta_tau_n) { + status_flags + = ((eph.M & 0x3) << 7) + | (eph.P4 ? 0x40 : 0) + | (eph.P2 ? 0x10 : 0) + | ((eph.P1_index() & 0x3) << 2); + health_flags = 0; } operator eph_t() const { typename GLONASS_SpaceNode::SatelliteProperties::Ephemeris eph = {0}; @@ -414,6 +433,12 @@ struct RINEX_NAV { eph.yn = 1E3 * y_km; eph.yn_dot = 1E3 * dy_km_s; eph.yn_ddot = 1E3 * ddy_km_s2; eph.zn = 1E3 * z_km; eph.zn_dot = 1E3 * dz_km_s; eph.zn_ddot = 1E3 * ddz_km_s2; eph.B_n = B_n; eph.E_n = E_n; + eph.F_T = eph_t::Ephemeris::raw_t::F_T_value(urai); + eph.delta_tau_n = (delta_tau > 0.999999999998E+09 ? 0 : delta_tau); + eph.M = ((status_flags >> 7) & 0x3); + eph.P4 = (status_flags & 0x40); + eph.P2 = (status_flags & 0x10); + eph.P1 = eph_t::Ephemeris::raw_t::P1_value((status_flags >> 2) & 0x03); return eph_t(eph, date_tm); } }; @@ -443,6 +468,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { static const typename super_t::convert_item_t eph1_glonass_v2[4], eph1_glonass_v3[4]; static const typename super_t::convert_item_t eph2_glonass_v2[4], eph2_glonass_v3[4]; static const typename super_t::convert_item_t eph3_glonass_v2[4], eph3_glonass_v3[4]; + static const typename super_t::convert_item_t eph4_glonass_v305[4]; protected: typename super_t::version_type_t::sat_system_t sys_of_msg; @@ -551,7 +577,9 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { msg_glonass.date_tm.tm_mon = msg_glonass.t_mon12 - 1; // month [0, 11] msg_glonass.t_sec = msg_glonass.date_tm.tm_sec; - for(int i(1); i < 4; i++){ + for(int i(1); + i < ((super_t::version_type.version <= 304) ? 4 : 5); + i++){ if((!super_t::src.good()) || super_t::src.getline(buf, sizeof(buf)).fail()){return;} std::string line(buf); @@ -560,6 +588,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { case 1: super_t::convert(eph1_glonass_v3, line, &msg_glonass); break; case 2: super_t::convert(eph2_glonass_v3, line, &msg_glonass); break; case 3: super_t::convert(eph3_glonass_v3, line, &msg_glonass); break; + case 4: super_t::convert(eph4_glonass_v305, line, &msg_glonass); break; } } sys_of_msg = super_t::version_type_t::SYS_GLONASS; @@ -1334,6 +1363,14 @@ const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader GEN_E2(61, 19, 12, message_glonass_t, E_n, unsigned int), }; +template +const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::eph4_glonass_v305[] = { + GEN_E2( 4, 19, 12, message_glonass_t, status_flags, unsigned int), + GEN_E (23, 19, 12, message_glonass_t, delta_tau), + GEN_E2(42, 19, 12, message_glonass_t, urai, unsigned int), + GEN_E2(61, 19, 12, message_glonass_t, health_flags, unsigned int), +}; + template const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader::iono_alpha_v2[] = { GEN_E( 2, 12, 4, iono_utc_t, alpha[0]), @@ -1749,13 +1786,16 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { } break; case 3: - for(int i(0); i < 4; ++i){ + for(int i(0); + i < ((super_t::_version_type.version <= 304) ? 4 : 5); + ++i){ std::string s(80, ' '); switch(i){ case 0: super_t::convert(reader_t::eph0_glonass_v3, s, &msg); s[0] = 'R'; break; case 1: super_t::convert(reader_t::eph1_glonass_v3, s, &msg); break; case 2: super_t::convert(reader_t::eph2_glonass_v3, s, &msg); break; case 3: super_t::convert(reader_t::eph3_glonass_v3, s, &msg); break; + case 4: super_t::convert(reader_t::eph4_glonass_v305, s, &msg); break; } buf << s << std::endl; } diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index 033d27fcd..cc585500a 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -391,6 +391,12 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ BOOST_CHECK_SMALL(std::abs( .215388488770E+04 - eph.zn_dot), 1E-08); BOOST_CHECK_SMALL(std::abs(-.186264514923E-05 - eph.zn_ddot), 1E-17); BOOST_CHECK_SMALL(std::abs( .100000000000E+01 - eph.E_n), 1E-11); + + // default setting until ver.3.05 + BOOST_CHECK_EQUAL(1, eph.M); // GLONASS-M + BOOST_CHECK_EQUAL(60 * 60, eph.P1); // 1 hour + BOOST_CHECK_EQUAL(-1, eph.F_T); // invalid range accuracy + BOOST_CHECK_EQUAL(0, eph.delta_tau_n); } std::string dist; @@ -418,6 +424,89 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ //compare_lines(src, dist, 9); // TODO check header } +BOOST_AUTO_TEST_CASE(nav_GLONASS_v305){ + const char *src = \ + " 3.05 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE\n" + "XXRINEXN V3 AIUB 20061002 000123 UTC PGM / RUN BY / DATE \n" + "EXAMPLE OF VERSION 3.05 FORMAT COMMENT \n" + "GPSA 0.1025E-07 0.7451E-08 -0.5960E-07 -0.5960E-07 IONOSPHERIC CORR \n" + "GPSB 0.8806E+05 0.0000E+00 -0.1966E+06 -0.6554E+05 IONOSPHERIC CORR \n" + "GPUT 0.2793967723E-08 0.000000000E+00 147456 1395 G10 2 TIME SYSTEM CORR \n" + "GLUT 0.7823109626E-06 0.000000000E+00 0 1395 R10 0 TIME SYSTEM CORR \n" + " 14 LEAP SECONDS \n" + " END OF HEADER \n" + "R01 2006 10 01 00 15 00-0.137668102980E-04-0.454747350886E-11 0.900000000000E+02\n" + " 0.157594921875E+05-0.145566368103E+01 0.000000000000E+00 0.000000000000E+00\n" + " -0.813711474609E+04 0.205006790161E+01 0.931322574615E-09 0.700000000000E+01\n" + " 0.183413398438E+05 0.215388488770E+01-0.186264514923E-08 0.100000000000E+01\n" + " 1.790000000000E+02 8.381903171539E-09 2.000000000000E+00 3.000000000000E+00\n"; + //----|---1|0---|---2|0---|---3|0---|---4|0---|---5|0---|---6|0---|---7|0---|---8| + + typedef RINEX_NAV_Reader reader_t; + typedef RINEX_NAV_Writer writer_t; + + check_reader_versatility_to_input(src); + + gps_t gps; + glonass_t glonass; + { + std::stringbuf sbuf(src); + std::istream in(&sbuf); + reader_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + reader_t::read_all(in, space_nodes); + } + + { + std::tm t_oc_tm = {0, 15, 0, 1, 10 - 1, 2006 - 1900}; + /*typename*/ gps_t::gps_time_t t_oc(t_oc_tm); + glonass.satellite(1).select_ephemeris(t_oc); + const /*typename*/ glonass_t::Satellite::Ephemeris &eph(glonass.satellite(1).ephemeris()); + + BOOST_CHECK_SMALL(std::abs( .137668102980E-04 - eph.tau_n), 1E-16); // tau_n is inverted + BOOST_CHECK_SMALL(std::abs(-.454747350886E-11 - eph.gamma_n), 1E-23); + BOOST_CHECK_SMALL(std::abs( .900000000000E+02 - eph.t_k), 1E-10); + + BOOST_CHECK_SMALL(std::abs( .157594921875E+08 - eph.xn), 1E-04); + BOOST_CHECK_SMALL(std::abs(-.145566368103E+04 - eph.xn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs( .000000000000E+03 - eph.xn_ddot), 1E-09); + BOOST_CHECK_SMALL(std::abs( .000000000000E+00 - eph.B_n), 1E-12); + + BOOST_CHECK_SMALL(std::abs(-.813711474609E+07 - eph.yn), 1E-05); + BOOST_CHECK_SMALL(std::abs( .205006790161E+04 - eph.yn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs( .931322574615E-06 - eph.yn_ddot), 1E-18); + BOOST_CHECK_SMALL(std::abs( .700000000000E+01 - eph.freq_ch), 1E-11); + + BOOST_CHECK_SMALL(std::abs( .183413398438E+08 - eph.zn), 1E-04); + BOOST_CHECK_SMALL(std::abs( .215388488770E+04 - eph.zn_dot), 1E-08); + BOOST_CHECK_SMALL(std::abs(-.186264514923E-05 - eph.zn_ddot), 1E-17); + BOOST_CHECK_SMALL(std::abs( .100000000000E+01 - eph.E_n), 1E-11); + + BOOST_CHECK_EQUAL(1, eph.M); // GLONASS-M + BOOST_CHECK_EQUAL(0, eph.P1); + BOOST_CHECK_EQUAL(2, eph.F_T_index()); + BOOST_CHECK_SMALL(std::abs(8.381903171539E-09 - eph.delta_tau_n), 1E-22); + } + + std::string dist; + { + std::stringstream ss; + writer_t writer(ss); + //writer.header()["PGM / RUN BY / DATE"] + // = "XXRINEXN V3 AIUB 20061002 000123 UTC"; + std::tm t_header = {23, 1, 0, 2, 10 - 1, 2006 - 1900}; + writer.pgm_runby_date("XXRINEXN V3", "AIUB", t_header, "UTC"); + writer.header()["COMMENT"] << "EXAMPLE OF VERSION 3.05 FORMAT"; + + writer_t::space_node_list_t space_nodes = {&gps}; + space_nodes.glonass = &glonass; + writer.write_all(space_nodes, 305); + dist = ss.str(); + } + + //compare_lines(src, dist, 9); // TODO check header +} + BOOST_AUTO_TEST_CASE(obs_GPS_v2){ const char *src = \ " 2.11 OBSERVATION DATA M (MIXED) RINEX VERSION / TYPE\n" From 70ed33e2a30520a3309b7a0fa1b18cb0646a0c98 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 10 Mar 2022 11:17:58 +0900 Subject: [PATCH 29/59] Change tau_GPS unit (day may be wrong) --- tool/navigation/GLONASS.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index ab13cd6b0..53f92fe8d 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -1219,7 +1219,7 @@ const typename GLONASS_SpaceNode::int_t GLONASS_SpaceNode::TimeP template const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::TimeProperties::raw_t::sf[] = { POWER_2(-31), // tau_c [s] - POWER_2(-30) * 60 * 60 * 24, // tau_GPS [day] => [s] + POWER_2(-30) /* * 60 * 60 * 24*/, // tau_GPS [s], unit is described as [day] in ICD, however, it may be wrong. }; template From 0626905f85624ef6e10dc26f31c2885b974a7e81 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 10 Mar 2022 13:51:33 +0900 Subject: [PATCH 30/59] Change GLONASS range correction to use GPS correction --- tool/swig/GPS.i | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 4886b1d03..6804970c5 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -1309,6 +1309,8 @@ struct GPS_Solver #ifdef SWIGRUBY hooks = rb_hash_new(); #endif + glonass.solver.ionospheric_correction.add(gps.solver.ionospheric_correction); + glonass.solver.tropospheric_correction.add(gps.solver.tropospheric_correction); } GPS_SpaceNode &gps_space_node() {return gps.space_node;} GPS_SolverOptions &gps_options() {return gps.options;} From 121a4bc93151e0513edbf239b599b0fd85d31dda Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Mar 2022 16:37:44 +0900 Subject: [PATCH 31/59] Fix time difference calculation in GLONASS --- tool/navigation/GLONASS.h | 46 +++++++++++++++++--------------- tool/navigation/GLONASS_Solver.h | 5 ++-- 2 files changed, 26 insertions(+), 25 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 53f92fe8d..08b023bf3 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -405,10 +405,10 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} constellation_t operator/(const float_t &sf) const { // for RK4 return operator*(((float_t)1)/sf); } - // TODO constant definitions should be moved to PZ-90.02 + // TODO constant definitions should be moved to PZ-90(.02, .11) static const float_t omega_E; constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ - // @see Appendix.A PZ-90.02 to O-X0Y0Z0 + // @see Appendix.A PZ-90 to O-X0Y0Z0 float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); float_t x0(position[0] * crad - position[1] * srad), @@ -424,7 +424,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return res; } constellation_t rel_corrdinate(const float_t &sidereal_time_in_rad){ - // @see Appendix.A O-X0Y0Z0 to PZ-90.02 + // @see Appendix.A O-X0Y0Z0 to PZ-90 float_t crad(std::cos(sidereal_time_in_rad)), srad(std::sin(sidereal_time_in_rad)); float_t x( position[0] * crad + position[1] * srad), @@ -927,10 +927,12 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} /** * * Relationship of time systems - * 1) t_UTC = t_GL + tau_c - 3hr (polarity of tau_c is not defined in ICD!) - * 2) t_GPS = t_GL + delta_T + tau_GPS (defined in ICD) - * 3) t_n(t_b) = t_GL(t_b) - tau_n (@(t_UTC + 3hr) = t_b, defined in ICD) - * 4) t_n = t_GL - tau_n + gamma_n * (t_GL - t_b) (not explicitly defined in ICD, but in RINEX spec.) + * 1) t_GL = t_UTC + 3hr (defined in ICD 3.3.3 GLONASS Time) + * 2) t_UTC + 3hr = t_rcv + tau_c + tau_n - gamma_n * (t_rcv - t_b) (defined in ICD 3.3.3 GLONASS Time, also in RINEX spec.) + * where tau_c: system wise, (-tau_c = GLUT in RINEX) + * ta_n, gamm_n: per satellite + * t_b along to UTC + 3hr (a.k.a, t_GL) + * 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.) */ struct Ephemeris_with_Time : public Ephemeris, TimeProperties { typedef typename Ephemeris::constellation_t constellation_t; @@ -984,15 +986,15 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const { delta_t -= pseudo_range / light_speed; - return -Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; + return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; } /** - * @param t_arrival_glonass signal arrival time in GLONASS time scale (t_GL = t_UTC + 3 hr + tau_c). + * @param t_arrival_onboard signal arrival time in onboard time scale, + * which is along to glonass time scale (UTC + 3 hr) but is shifted in gradually changing length. */ float_t clock_error( - const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const { - return calculate_clock_error( - t_arrival_glonass + TimeProperties::tau_c - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale + const float_t &t_arrival_onboard, const float_t &pseudo_range = 0) const { + return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range); } /** * Calculate constellation(t) based on constellation(t_0). @@ -1036,12 +1038,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return calculate_constellation(delta_t, pseudo_range, xa_t_b, float_t(0)); } /** - * @param t_arrival_glonass signal arrival time in GLONASS time scale (t_GL = t_UTC + 3 hr + tau_c). + * @param t_arrival_glonass signal arrival time in glonass time scale, + * which is the corrected onboard time by removing clock error. */ constellation_t constellation( const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const { return calculate_constellation( - t_arrival_glonass + TimeProperties::tau_c - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale + t_arrival_glonass - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale } }; @@ -1057,10 +1060,9 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} */ Ephemeris_with_GPS_Time(const Ephemeris_with_Time &eph, const int_t &deltaT = 0) : Ephemeris_with_Time(eph), - t_b_gps((GPS_Time(eph.c_tm_utc()) // in UTC scale - + (-Ephemeris_with_Time::tau_c // in Moscow Time scale - + Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer) - + deltaT))) { + t_b_gps(GPS_Time(eph.c_tm_utc()) // in UTC scale + + Ephemeris_with_Time::tau_GPS // in (GPS - delta_T) scale (delta_T is integer), -tau_GPS = GLGP in RINEX + + deltaT) { } GPS_Time base_time() const { return t_b_gps; @@ -1070,13 +1072,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } using Ephemeris_with_Time::clock_error; float_t clock_error( - const GPS_Time &t_arrival, const float_t &pseudo_range = 0) const { - return Ephemeris_with_Time::calculate_clock_error(t_arrival - t_b_gps, pseudo_range); + const GPS_Time &t_arrival_onboard, const float_t &pseudo_range = 0) const { + return Ephemeris_with_Time::calculate_clock_error(t_arrival_onboard - t_b_gps, pseudo_range); } using Ephemeris_with_Time::constellation; typename Ephemeris_with_Time::constellation_t constellation( - const GPS_Time &t_arrival, const float_t &pseudo_range = 0) const { - return this->calculate_constellation(t_arrival - t_b_gps, pseudo_range); + const GPS_Time &t_arrival_gps, const float_t &pseudo_range = 0) const { + return this->calculate_constellation(t_arrival_gps - t_b_gps, pseudo_range); } }; }; diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 7dd797d69..7d6de4f12 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -248,11 +248,10 @@ class GLONASS_SinglePositioning : public SolverBaseT { satellite_t sat(select_satellite(prn, time_arrival)); if(!sat.is_available()){return res;} // If satellite is unavailable, return with weight = 0 - ///< The following procedure is based on Appendix.S with modification - range -= receiver_error; - // Clock correction will be performed in the following constellation() + // Clock correction, which will be considered in the next constellation() + // as extra transmission time by using extra psuedo range. if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){ range += (sat.clock_error(time_arrival, range) * space_node_t::light_speed); }else{ From f0258f9899e913405f694ea527cdc16a7ea701e6 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 16 Mar 2022 16:43:23 +0900 Subject: [PATCH 32/59] Update GLONASS corrdinate system from PZ-90.02 to PZ-90.11 --- tool/navigation/GLONASS.h | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 08b023bf3..2b55fb8ce 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -441,6 +441,17 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } operator typename GPS_SpaceNode::SatelliteProperties ::constellation_t() const { +#if 1 + // @see https://gssc.esa.int/navipedia/index.php/Reference_Frames_in_GNSS + // PZ90.11 -> WGS84 (starting from 3:00 pm on December 31, 2013) + typename GPS_SpaceNode::SatelliteProperties::constellation_t res = { + typename GPS_SpaceNode::xyz_t( + position[0] + 0.003, position[1] + 0.001, position[2] + 0.001), + typename GPS_SpaceNode::xyz_t( + velocity[0], velocity[1], velocity[2]), + }; + return res; +#else // @see https://www.gsi.go.jp/common/000070971.pdf // @see (originally) Federal Air Navigation Authority (FANA), Aeronautical Information Circular // of the Russian Federation, 12 February 2009, Russia. @@ -452,6 +463,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} velocity[0], velocity[1], velocity[2]), }; return res; +#endif } }; From 24993562c8dcc61079d8c4f35860ab830c703753 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 17 Mar 2022 15:24:11 +0900 Subject: [PATCH 33/59] Add const qualifier to GLONASS space node of RINEX reader input --- tool/navigation/RINEX.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 46f9594e1..153ff4949 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -1815,7 +1815,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { struct space_node_list_t { const space_node_t *gps; - GLONASS_SpaceNode *glonass; + const GLONASS_SpaceNode *glonass; }; int write_all( const space_node_list_t &space_nodes, From dfb311c30c5c80794bf2a48952b7842980547926 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 17 Mar 2022 14:01:28 +0900 Subject: [PATCH 34/59] Remove debug feature before merge --- tool/misc/receiver_debug.rb | 72 ++----------------------------------- 1 file changed, 2 insertions(+), 70 deletions(-) diff --git a/tool/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index d8c858a13..914e6695a 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -31,12 +31,11 @@ def self.pvt_items(opt = {}) [:week, :seconds, :utc].collect{|f| pvt.receiver_time.send(f)}.flatten } ]] + [[ - [:receiver_clock_error_meter, :delta_system, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U], + [:receiver_clock_error_meter, :longitude, :latitude, :height, :rel_E, :rel_N, :rel_U], proc{|pvt| - next [nil] * 8 unless pvt.position_solved? + next [nil] * 7 unless pvt.position_solved? [ pvt.receiver_error, - pvt.other_state[0], pvt.llh.lng / Math::PI * 180, pvt.llh.lat / Math::PI * 180, pvt.llh.alt, @@ -274,7 +273,6 @@ def initialize(options = {}) labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"} update_output.call(:GLONASS, prns, labels) prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)} - #$stderr.puts @solver.glonass_options.excluded.inspect else raise "Unknown satellite: #{spec}" end @@ -731,57 +729,6 @@ def attach_antex(fname) } }.call if [:start_time, :end_time].any?{|k| misc_options[k]} - rcv.solver.hooks[:update_position_solution] = proc{|mat_G, mat_W, mat_r, tmp_pvt| - i_glonass = [] - i_other = [] - tmp_pvt.used_satellite_list.each.with_index{|prn, i| - ((prn & 0x100 == 0x100) ? i_glonass : i_other) << i - } - next if (i_glonass.empty? || i_other.empty?) - mat_G.resize!(nil, 5) - i_glonass.each{|i| mat_G[i, 4] = 1} - } - - glonass_residuals = {} # => {time => {svid => [residual, other_props]}, ...} - proc{ - orig_hook = rcv.solver.hooks[:relative_property] - rcv.solver.hooks[:relative_property] = proc{|prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel| - rel_prop = orig_hook.call(prn, rel_prop, meas, rcv_e, t_arv, usr_pos, usr_vel) if orig_hook - weight, range_c, range_r, rate_rel_neg, *los_neg = rel_prop # relative property - - while (prn & 0x100) == 0x100 # GLONASS - svid = prn - 0x100 - eph = rcv.solver.glonass_space_node.ephemeris(svid) - break if (eph.base_time - t_arv).abs >= 60 * 60 * 2 - break unless range = meas[GPS::Measurement::L1_PSEUDORANGE] - range -= rcv_e - clk_error = eph.clock_error(t_arv, range) * GPS::SpaceNode::light_speed - range += clk_error - sat_pos = eph.constellation(t_arv, range)[0] - rel_pos = sat_pos - usr_pos - rel_pos_enu = Coordinate::ENU::relative_rel(rel_pos, usr_pos) - range -= rel_pos.dist - sn = rcv.solver.gps_space_node - # GPS ICD 20.3.3.3.3.2 L1 - L2 Correction. - gamma = GPS::SpaceNode.gamma_per_L1(eph.frequency_L1) - iono = gamma * sn.iono_correction(rel_pos_enu, usr_pos.llh, t_arv) - tropo = GPS::SpaceNode::tropo_correction(sat_pos, usr_pos) - residual = range + iono + tropo - el, az = [:elevation, :azimuth].collect{|f| rel_pos_enu.send(f) / Math::PI * 180} - - t_arv = [t_arv.week, t_arv.seconds.round(1)] # week, ms - glonass_residuals[t_arv] ||= {} - if (!glonass_residuals[t_arv][svid]) \ - || (glonass_residuals[t_arv][svid][2].abs > residual.abs) then - glonass_residuals[t_arv][svid] = [weight, range_r, residual, el, az, clk_error, iono, tropo] - end - break - end - - [weight, range_c, range_r, rate_rel_neg] + los_neg # must return relative property - } - }.call - puts rcv.header # parse RINEX NAV, SP3, or ANTEX @@ -800,19 +747,4 @@ def attach_antex(fname) when :rinex_obs; rcv.parse_rinex_obs(fname) end } - - open("glonass.csv", 'w'){|io| - io.puts(([:week, :seconds_in_week, - :year, :month, :day, :hour, :min, :seconds] + (1..24).collect{|svid| - [:weight_orig, :residual_orig, :residual, :el, :az, :clk_error, :iono, :tropo].collect{|k| - "#{k}(%d)"%[svid] - } - }).flatten.join(',')) - glonass_residuals.to_a.sort{|a, b| a[0] <=> b[0]}.each{|t, sv_props| - t = GPS::Time::new(*t[0..1]) - io.puts(([t.week, "%.3f"%[t.seconds]] + t.c_tm.to_a + (1..24).collect{|svid| - sv_props[svid] || ([nil] * 8) - }).flatten.join(',')) - } - } end From 2a1478465cf2a2561e8df9561e5ab783225b89d1 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 24 Mar 2022 11:29:31 +0900 Subject: [PATCH 35/59] Add integration cache to calculate GLONASS constellation --- tool/navigation/GLONASS.h | 62 ++++++++++++++++++++++++++------------- 1 file changed, 42 insertions(+), 20 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 2b55fb8ce..b3a97c65f 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -1009,36 +1009,31 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range); } /** - * Calculate constellation(t) based on constellation(t_0). + * Calculate absolute constellation(t) based on constellation(t_0). * t_0 is a time around t_b, and is used to calculate - * an intermediate result specified with the 3rd argument. + * an intermediate result specified with the 2nd argument. * This method is useful to calculate constellation effectively * by using a cache. * @param delta_t time interval from t_0 to t - * @param pseudo_range measured pusedo_range to correct delta_t * @param xa_t_0 constellation(t_0) - * @param t_0_from_t_b time interval from t_b to t_0 + * @param t_0_from_t_b time interval from t_b to t_0 (= t_0 - t_b) */ - constellation_t calculate_constellation( - float_t delta_t, const float_t &pseudo_range, + constellation_t constellation_abs( + const float_t &delta_t, const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { constellation_t res(xa_t_0); - { // time integration from t_b to t_transmit - float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed); - float_t t_step_max(delta_t_to_transmit >= 0 ? 60 : -60); - int i(std::floor(delta_t_to_transmit / t_step_max)); - float_t t_step_remain(delta_t_to_transmit - t_step_max * i); + { // time integration from t_0 to (t_0 + delta_t) + float_t t_step_max(delta_t >= 0 ? 60 : -60); + int i(std::floor(delta_t / t_step_max)); + float_t t_step_remain(delta_t - t_step_max * i); float_t delta_t_itg(0); // accumulative time of integration for(; i > 0; --i, delta_t_itg += t_step_max){ res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_max); } res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_remain); } - - static const float_t omega_E(0.7292115E-4); // Earth's rotation rate, TODO move to PZ-90.02 - return res.rel_corrdinate( - sidereal_t_b_rad + (omega_E * (delta_t + t_0_from_t_b))); // transform from abs to PZ-90.02 + return res; } /** * Calculate constellation(t) based on constellation(t_b). @@ -1046,8 +1041,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} * @param pseudo_range measured pusedo_range to correct delta_t, default is 0. */ constellation_t calculate_constellation( - float_t delta_t, const float_t &pseudo_range = 0) const { - return calculate_constellation(delta_t, pseudo_range, xa_t_b, float_t(0)); + const float_t &delta_t, const float_t &pseudo_range = 0) const { + + float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed); + constellation_t res( + constellation_abs(delta_t_to_transmit, xa_t_b, float_t(0))); + + return res.rel_corrdinate( + sidereal_t_b_rad + (constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 } /** * @param t_arrival_glonass signal arrival time in glonass time scale, @@ -1098,7 +1099,28 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} struct Satellite : public SatelliteProperties { public: typedef typename SatelliteProperties::Ephemeris_with_GPS_Time eph_t; - typedef typename GPS_SpaceNode::template PropertyHistory eph_list_t; + + struct eph_cached_t : public eph_t { + mutable typename eph_t::constellation_t xa_t_0; + mutable float_t t_0_from_t_b; + eph_cached_t() : eph_t(), xa_t_0(eph_t::xa_t_b), t_0_from_t_b(0) {} + eph_cached_t(const eph_t &eph) : eph_t(eph), xa_t_0(eph.xa_t_b), t_0_from_t_b(0) {} + using eph_t::constellation; + typename eph_t::constellation_t constellation( + const GPS_Time &t_arrival_gps, const float_t &pseudo_range = 0) const { + float_t delta_t(t_arrival_gps - eph_t::t_b_gps); + float_t delta_t_transmit_from_t_0(delta_t - pseudo_range / light_speed - t_0_from_t_b); + + // perform integration and update cache + xa_t_0 = eph_t::constellation_abs(delta_t_transmit_from_t_0, xa_t_0, t_0_from_t_b); + t_0_from_t_b += delta_t_transmit_from_t_0; + + return xa_t_0.rel_corrdinate( + eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 + } + }; + + typedef typename GPS_SpaceNode::template PropertyHistory eph_list_t; protected: eph_list_t eph_history; public: @@ -1115,7 +1137,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} } void register_ephemeris(const eph_t &eph, const int &priority_delta = 1){ - eph_history.add(eph, priority_delta); + eph_history.add(eph_cached_t(eph), priority_delta); } void merge(const Satellite &another, const bool &keep_original = true){ @@ -1143,7 +1165,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} typename GPS_SpaceNode::SatelliteProperties::constellation_t constellation( const GPS_Time &t, const float_t &pseudo_range = 0) const { return (typename GPS_SpaceNode::SatelliteProperties::constellation_t)( - ephemeris().constellation(t, pseudo_range)); + eph_history.current().constellation(t, pseudo_range)); } typename GPS_SpaceNode::xyz_t position(const GPS_Time &t, const float_t &pseudo_range = 0) const { From 3be96dc3a71608836d0fef691e96861e65fd12ae Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 30 Mar 2022 10:01:39 +0900 Subject: [PATCH 36/59] Change update policy of GLONASS constellation cache --- tool/navigation/GLONASS.h | 71 +++++++++++++++++++++++++++++---------- 1 file changed, 54 insertions(+), 17 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index b3a97c65f..2b5f50a9c 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -952,6 +952,8 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} float_t sidereal_t_b_rad; typename Ephemeris::differential_t eq_of_motion; + static const float_t time_step_max_per_one_integration; + void calculate_additional() { sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg( TimeProperties::date.c_tm(), @@ -1008,6 +1010,26 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} const float_t &t_arrival_onboard, const float_t &pseudo_range = 0) const { return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range); } + /** + * Calculate absolute constellation(t) based on constellation(t_0). + * t_0 is a time around t_b, and is used to calculate + * @param t_step time interval from t_0 to t + * @param times number of integration steps (assume times >=0) + * @param xa_t_0 constellation(t_0) + * @param t_0_from_t_b time interval from t_b to t_0 (= t_0 - t_b) + */ + constellation_t constellation_abs( + const float_t &t_step, + int times, + const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { + + constellation_t res(xa_t_0); + float_t delta_t_itg(t_0_from_t_b); // accumulative time of integration + for(; times > 0; --times, delta_t_itg += t_step){ + res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step); + } + return res; + } /** * Calculate absolute constellation(t) based on constellation(t_0). * t_0 is a time around t_b, and is used to calculate @@ -1022,18 +1044,19 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} const float_t &delta_t, const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { - constellation_t res(xa_t_0); - { // time integration from t_0 to (t_0 + delta_t) - float_t t_step_max(delta_t >= 0 ? 60 : -60); - int i(std::floor(delta_t / t_step_max)); - float_t t_step_remain(delta_t - t_step_max * i); - float_t delta_t_itg(0); // accumulative time of integration - for(; i > 0; --i, delta_t_itg += t_step_max){ - res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_max); - } - res = nextByRK4(eq_of_motion, delta_t_itg, res, t_step_remain); - } - return res; + float_t t_step_max(delta_t >= 0 + ? time_step_max_per_one_integration + : -time_step_max_per_one_integration); + int n(std::floor(delta_t / t_step_max)); + float_t delta_t_steps(t_step_max * n), delta_t_remain(delta_t - delta_t_steps); + + // To perform time integration from t_0 to (t_0 + delta_t), + // n-times integration with t_step_max, + // then, one-time integration with delta_t_remain are conducted. + return constellation_abs( + delta_t_remain, 1, + constellation_abs(t_step_max, n, xa_t_0, t_0_from_t_b), + t_0_from_t_b + delta_t_steps); } /** * Calculate constellation(t) based on constellation(t_b). @@ -1111,12 +1134,21 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} float_t delta_t(t_arrival_gps - eph_t::t_b_gps); float_t delta_t_transmit_from_t_0(delta_t - pseudo_range / light_speed - t_0_from_t_b); - // perform integration and update cache - xa_t_0 = eph_t::constellation_abs(delta_t_transmit_from_t_0, xa_t_0, t_0_from_t_b); - t_0_from_t_b += delta_t_transmit_from_t_0; + float_t t_step_max(delta_t_transmit_from_t_0 >= 0 + ? eph_t::time_step_max_per_one_integration + : -eph_t::time_step_max_per_one_integration); + + int big_steps(std::floor(delta_t_transmit_from_t_0 / t_step_max)); + if(big_steps > 0){ // perform integration and update cache + xa_t_0 = eph_t::constellation_abs(t_step_max, big_steps, xa_t_0, t_0_from_t_b); + float_t delta_t_updated(t_step_max * big_steps); + t_0_from_t_b += delta_t_updated; + delta_t_transmit_from_t_0 -= delta_t_updated; + } - return xa_t_0.rel_corrdinate( - eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 + return eph_t::constellation_abs(delta_t_transmit_from_t_0, 1, xa_t_0, t_0_from_t_b) + .rel_corrdinate( + eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 } }; @@ -1289,4 +1321,9 @@ typename GLONASS_SpaceNode::u8_t GLONASS_SpaceNode::SatellitePro return res; } +template +const typename GLONASS_SpaceNode::float_t + GLONASS_SpaceNode::SatelliteProperties::Ephemeris_with_Time + ::time_step_max_per_one_integration = 60; + #endif /* __GLONASS_H__ */ From 3938cd80c0c72df6e7d02762f62aea866473b679 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 9 Jul 2022 07:59:34 +0900 Subject: [PATCH 37/59] Add GLONASS SP3 support --- tool/INS_GPS/GNSS_Receiver.h | 2 ++ tool/swig/GPS.i | 5 ++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 83cf01c59..7353af349 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -179,6 +179,7 @@ struct GNSS_Receiver { #if !defined(BUILD_WITHOUT_SP3) typename data_t::sp3_t::satellite_count_t cnt(data.sp3.satellite_count()); if(cnt.gps > 0){data.sp3.push(gps.satellites, SP3_Product::SYSTEM_GPS);} + if(cnt.glonass > 0){data.sp3.push(glonass.satellites, SP3_Product::SYSTEM_GLONASS);} #endif } } solver_GNSS; @@ -390,6 +391,7 @@ struct GNSS_Receiver { std::cerr << "sp3: " << entries << " items captured." << std::endl; typename data_t::sp3_t::satellite_count_t cnt(data.sp3.satellite_count()); if(cnt.gps > 0){std::cerr << "SP3 GPS satellites: " << cnt.gps << std::endl;} + if(cnt.glonass > 0){std::cerr << "SP3 GLONASS satellites: " << cnt.glonass << std::endl;} } solver_GNSS.update_ephemeris_source(data); return true; diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 6804970c5..e590f36dc 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -1555,7 +1555,10 @@ struct SP3 : public SP3_Product { solver.gps.solver.satellites, SP3_Product::SYSTEM_GPS); case SYS_SBAS: case SYS_QZSS: + break; case SYS_GLONASS: + return SP3_Product::push( + solver.glonass.solver.satellites, SP3_Product::SYSTEM_GLONASS); case SYS_GALILEO: case SYS_BEIDOU: default: @@ -1568,7 +1571,7 @@ struct SP3 : public SP3_Product { SYS_GPS, //SYS_SBAS, //SYS_QZSS, - //SYS_GLONASS, + SYS_GLONASS, //SYS_GALILEO, //SYS_BEIDOU, }; From dc0246d52e1ac58fc32a8489fab32204246a71f5 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 21 Jul 2022 15:11:10 +0900 Subject: [PATCH 38/59] Add leap second guess when it is not provided in GLONASS RINEX NAV --- tool/navigation/RINEX.h | 32 ++++++++++++++++++++++---------- tool/test/test_RINEX.cpp | 4 ++-- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 153ff4949..874302341 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -726,29 +726,37 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { int year, month, day; FloatT tau_c_neg, tau_GPS; // TODO check tau_GPS polarity int leap_sec; + int flags; + enum { + TAU_C_NEG = 0x01, + TAU_GPS = 0x02, + LEAP_SEC = 0x04, + }; }; static const typename super_t::convert_item_t t_corr_glonass_v2[4]; bool extract_t_corr_glonass_v2(t_corr_glonass_t &t_corr_glonass) const { - bool utc, leap; + t_corr_glonass.flags = 0; super_t::header_t::const_iterator it; - if(utc = ((it = _header.find("CORR TO SYSTEM TIME")) != _header.end())){ + if((it = _header.find("CORR TO SYSTEM TIME")) != _header.end()){ super_t::convert(t_corr_glonass_v2, it->second.front(), &t_corr_glonass); + t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG; } - if(leap = ((it = _header.find("LEAP SECONDS")) != _header.end())){ + if((it = _header.find("LEAP SECONDS")) != _header.end()){ iono_utc_t iono_utc; super_t::convert(utc_leap_v2, it->second.front(), &iono_utc); t_corr_glonass.leap_sec = iono_utc.delta_t_LS; + t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC; } - return utc && leap; + return t_corr_glonass.flags > 0; } bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const { iono_utc_t iono_utc; - bool utc(false), leap(false); + t_corr_glonass.flags = 0; typedef super_t::header_t::const_iterator it_t; typedef super_t::header_t::mapped_type::const_iterator it2_t; @@ -760,11 +768,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { super_t::convert(utc_v3, *it2, &iono_utc); t_corr_glonass.year = t_corr_glonass.month = t_corr_glonass.day = 0; t_corr_glonass.tau_c_neg = iono_utc.A0; + t_corr_glonass.flags |= t_corr_glonass_t::TAU_C_NEG; }else if(it2->find("GLGP") != it2->npos){ super_t::convert(utc_v3, *it2, &iono_utc); t_corr_glonass.tau_GPS = iono_utc.A0; + t_corr_glonass.flags |= t_corr_glonass_t::TAU_GPS; } - utc = true; } } @@ -775,10 +784,10 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { super_t::convert(utc_leap_v2, it->second.front(), &iono_utc); } t_corr_glonass.leap_sec = iono_utc.delta_t_LS; - leap = true; + t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC; } - return utc && leap; + return t_corr_glonass.flags > 0; } struct space_node_list_t { @@ -815,8 +824,11 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { typename message_glonass_t::eph_t eph0(reader.msg_glonass); eph0.tau_c = -t_corr_glonass.tau_c_neg; eph0.tau_GPS = t_corr_glonass.tau_GPS; - typename GLONASS_SpaceNode - ::SatelliteProperties::Ephemeris_with_GPS_Time eph(eph0, t_corr_glonass.leap_sec); + typename GLONASS_SpaceNode::SatelliteProperties::Ephemeris_with_GPS_Time eph( + eph0, + (t_corr_glonass.flags & t_corr_glonass_t::LEAP_SEC) + ? t_corr_glonass.leap_sec + : GPS_Time::guess_leap_seconds(reader.msg_glonass.date_tm)); space_nodes.glonass->satellite(reader.msg_glonass.svid).register_ephemeris(eph); res++; break; diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index cc585500a..154d2651e 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -293,7 +293,7 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v2){ { std::tm t_tm = {0, 15, 0, 15, 2 - 1, 1998 - 1900}; - /*typename*/ gps_t::gps_time_t t(t_tm); + /*typename*/ gps_t::gps_time_t t(t_tm, 12); glonass.satellite(3).select_ephemeris(t); const /*typename*/ glonass_t::Satellite::Ephemeris &eph(glonass.satellite(3).ephemeris()); @@ -369,7 +369,7 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ { std::tm t_oc_tm = {0, 15, 0, 1, 10 - 1, 2006 - 1900}; - /*typename*/ gps_t::gps_time_t t_oc(t_oc_tm); + /*typename*/ gps_t::gps_time_t t_oc(t_oc_tm, 14); glonass.satellite(1).select_ephemeris(t_oc); const /*typename*/ glonass_t::Satellite::Ephemeris &eph(glonass.satellite(1).ephemeris()); From 85a62692065796598871a56ddb04a2b7f52f147d Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 2 Aug 2022 09:11:31 +0900 Subject: [PATCH 39/59] Change constellation() argument to transmission time --- tool/navigation/GLONASS.h | 66 +++++++++++++++----------------- tool/navigation/GLONASS_Solver.h | 26 +++++++------ tool/swig/GPS.i | 24 ++++-------- 3 files changed, 53 insertions(+), 63 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 2b5f50a9c..44b5c784b 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -998,17 +998,15 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} bool is_equivalent(const Ephemeris_with_Time &eph) const { return Ephemeris::is_equivalent(eph) && TimeProperties::is_equivalent(eph); } - float_t calculate_clock_error(float_t delta_t, const float_t &pseudo_range = 0) const { - delta_t -= pseudo_range / light_speed; + float_t calculate_clock_error(const float_t &delta_t) const { return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; } /** - * @param t_arrival_onboard signal arrival time in onboard time scale, + * @param t_depature_onboard signal depature time in onboard time scale, * which is along to glonass time scale (UTC + 3 hr) but is shifted in gradually changing length. */ - float_t clock_error( - const float_t &t_arrival_onboard, const float_t &pseudo_range = 0) const { - return calculate_clock_error(t_arrival_onboard - Ephemeris::t_b, pseudo_range); + float_t clock_error(const float_t &t_depature_onboard) const { + return calculate_clock_error(t_depature_onboard - Ephemeris::t_b); } /** * Calculate absolute constellation(t) based on constellation(t_0). @@ -1064,23 +1062,22 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} * @param pseudo_range measured pusedo_range to correct delta_t, default is 0. */ constellation_t calculate_constellation( - const float_t &delta_t, const float_t &pseudo_range = 0) const { + const float_t &delta_t, const float_t &dt_transit = 0) const { - float_t delta_t_to_transmit(delta_t - pseudo_range / light_speed); constellation_t res( - constellation_abs(delta_t_to_transmit, xa_t_b, float_t(0))); + constellation_abs(delta_t, xa_t_b, float_t(0))); return res.rel_corrdinate( - sidereal_t_b_rad + (constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 + sidereal_t_b_rad + (constellation_t::omega_E * (delta_t + dt_transit))); // transform from abs to PZ-90.02 } /** - * @param t_arrival_glonass signal arrival time in glonass time scale, + * @param t_depature_glonass signal depature time in glonass time scale, * which is the corrected onboard time by removing clock error. */ constellation_t constellation( - const float_t &t_arrival_glonass, const float_t &pseudo_range = 0) const { + const float_t &t_depature_glonass, const float_t &dt_transit = 0) const { return calculate_constellation( - t_arrival_glonass - Ephemeris::t_b, pseudo_range); // measure in UTC + 3hr scale + t_depature_glonass - Ephemeris::t_b, dt_transit); // measure in UTC + 3hr scale } }; @@ -1107,14 +1104,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return std::abs(t_b_gps.interval(t)) <= 60 * 60; // 1 hour } using Ephemeris_with_Time::clock_error; - float_t clock_error( - const GPS_Time &t_arrival_onboard, const float_t &pseudo_range = 0) const { - return Ephemeris_with_Time::calculate_clock_error(t_arrival_onboard - t_b_gps, pseudo_range); + float_t clock_error(const GPS_Time &t_depature_onboard) const { + return Ephemeris_with_Time::calculate_clock_error(t_depature_onboard - t_b_gps); } using Ephemeris_with_Time::constellation; typename Ephemeris_with_Time::constellation_t constellation( - const GPS_Time &t_arrival_gps, const float_t &pseudo_range = 0) const { - return this->calculate_constellation(t_arrival_gps - t_b_gps, pseudo_range); + const GPS_Time &t_depature_gps, const float_t &dt_transit = 0) const { + return this->calculate_constellation(t_depature_gps - t_b_gps, dt_transit); } }; }; @@ -1130,25 +1126,25 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} eph_cached_t(const eph_t &eph) : eph_t(eph), xa_t_0(eph.xa_t_b), t_0_from_t_b(0) {} using eph_t::constellation; typename eph_t::constellation_t constellation( - const GPS_Time &t_arrival_gps, const float_t &pseudo_range = 0) const { - float_t delta_t(t_arrival_gps - eph_t::t_b_gps); - float_t delta_t_transmit_from_t_0(delta_t - pseudo_range / light_speed - t_0_from_t_b); + const GPS_Time &t_depature_gps, const float_t &dt_transit = 0) const { + float_t delta_t(t_depature_gps - eph_t::t_b_gps); + float_t delta_t_depature_from_t_0(delta_t - t_0_from_t_b); - float_t t_step_max(delta_t_transmit_from_t_0 >= 0 + float_t t_step_max(delta_t_depature_from_t_0 >= 0 ? eph_t::time_step_max_per_one_integration : -eph_t::time_step_max_per_one_integration); - int big_steps(std::floor(delta_t_transmit_from_t_0 / t_step_max)); + int big_steps(std::floor(delta_t_depature_from_t_0 / t_step_max)); if(big_steps > 0){ // perform integration and update cache xa_t_0 = eph_t::constellation_abs(t_step_max, big_steps, xa_t_0, t_0_from_t_b); float_t delta_t_updated(t_step_max * big_steps); t_0_from_t_b += delta_t_updated; - delta_t_transmit_from_t_0 -= delta_t_updated; + delta_t_depature_from_t_0 -= delta_t_updated; } - return eph_t::constellation_abs(delta_t_transmit_from_t_0, 1, xa_t_0, t_0_from_t_b) - .rel_corrdinate( - eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * delta_t)); // transform from abs to PZ-90.02 + return eph_t::constellation_abs(delta_t_depature_from_t_0, 1, xa_t_0, t_0_from_t_b) + .rel_corrdinate( // transform from abs to PZ-90.02 + eph_t::sidereal_t_b_rad + (eph_t::constellation_t::omega_E * (delta_t + dt_transit))); } }; @@ -1190,22 +1186,22 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return eph_history.select(target_time, &eph_t::is_valid); } - float_t clock_error(const GPS_Time &t, const float_t &pseudo_range = 0) const{ - return ephemeris().clock_error(t, pseudo_range); + float_t clock_error(const GPS_Time &t_tx) const{ + return ephemeris().clock_error(t_tx); } typename GPS_SpaceNode::SatelliteProperties::constellation_t constellation( - const GPS_Time &t, const float_t &pseudo_range = 0) const { + const GPS_Time &t_tx, const float_t &dt_transit = 0) const { return (typename GPS_SpaceNode::SatelliteProperties::constellation_t)( - eph_history.current().constellation(t, pseudo_range)); + eph_history.current().constellation(t_tx, dt_transit)); } - typename GPS_SpaceNode::xyz_t position(const GPS_Time &t, const float_t &pseudo_range = 0) const { - return constellation(t, pseudo_range).position; + typename GPS_SpaceNode::xyz_t position(const GPS_Time &t_tx, const float_t &dt_transit= 0) const { + return constellation(t_tx, dt_transit).position; } - typename GPS_SpaceNode::xyz_t velocity(const GPS_Time &t, const float_t &pseudo_range = 0) const { - return constellation(t, pseudo_range).velocity; + typename GPS_SpaceNode::xyz_t velocity(const GPS_Time &t_tx, const float_t &dt_transit = 0) const { + return constellation(t_tx, dt_transit).velocity; } }; public: diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 7d6de4f12..7ee9c4598 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -119,16 +119,16 @@ class GLONASS_SinglePositioning : public SolverBaseT { static inline const typename space_node_t::Satellite &sat(const void *ptr) { return *reinterpret_cast(ptr); } - static xyz_t position(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { - return sat(ptr).constellation(t, pseudo_range).position; + static xyz_t position(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) { + return sat(ptr).constellation(t_tx, dt_transit).position; } - static xyz_t velocity(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { - return sat(ptr).constellation(t, pseudo_range).velocity; + static xyz_t velocity(const void *ptr, const gps_time_t &t_tx, const float_t &dt_transit) { + return sat(ptr).constellation(t_tx, dt_transit).velocity; } - static float_t clock_error(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { - return sat(ptr).clock_error(t, pseudo_range); + static float_t clock_error(const void *ptr, const gps_time_t &t_tx) { + return sat(ptr).clock_error(t_tx); } - static float_t clock_error_dot(const void *ptr, const gps_time_t &t, const float_t &pseudo_range) { + static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) { // Clock rate error is already taken into account in constellation() return 0; } @@ -250,16 +250,20 @@ class GLONASS_SinglePositioning : public SolverBaseT { range -= receiver_error; + static const float_t &c(space_node_t::light_speed); + // Clock correction, which will be considered in the next constellation() // as extra transmission time by using extra psuedo range. if(range_error.unknown_flag & range_error_t::SATELLITE_CLOCK){ - range += (sat.clock_error(time_arrival, range) * space_node_t::light_speed); + range += (sat.clock_error(time_arrival - range / c) * c); }else{ range += range_error.value[range_error_t::SATELLITE_CLOCK]; } // Calculate satellite position - xyz_t sat_pos(sat.position(time_arrival, range)); + float_t dt_transit(range / c); + gps_time_t t_tx(time_arrival - dt_transit); + xyz_t sat_pos(sat.position(t_tx, dt_transit)); float_t geometric_range(usr_pos.xyz.dist(sat_pos)); // Calculate residual @@ -307,12 +311,12 @@ class GLONASS_SinglePositioning : public SolverBaseT { res.range_corrected = range; - xyz_t rel_vel(sat.velocity(time_arrival, range) - usr_vel); // Calculate velocity + xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity res.rate_relative_neg = res.los_neg[0] * rel_vel.x() + res.los_neg[1] * rel_vel.y() + res.los_neg[2] * rel_vel.z() - + sat.clock_error_dot(time_arrival, range) * space_node_t::light_speed; + + sat.clock_error_dot(t_tx) * c; return res; } diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 31e093b59..42d2c52d2 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -543,23 +543,13 @@ struct GLONASS_Ephemeris self->has_string = has_string; return updated; } - FloatT clock_error( - const GPS_Time &t_arrival, const FloatT &pseudo_range = 0) const { - return self->clock_error(t_arrival, pseudo_range); - } - %typemap(in,numinputs=0) System_XYZ & (System_XYZ temp) %{ - $1 = &temp; - %} - %typemap(argout) System_XYZ & { - %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN)); - } - void constellation( - System_XYZ &position, System_XYZ &velocity, - const GPS_Time &t, const FloatT &pseudo_range = 0) const { - typename GPS_SpaceNode::SatelliteProperties::constellation_t res( - self->constellation(t, pseudo_range)); - position = res.position; - velocity = res.velocity; + typename GPS_Ephemeris::constellation_res_t constellation( + const GPS_Time &t_tx, const FloatT &dt_transit = 0) const { + typename GPS_SpaceNode::SatelliteProperties::constellation_t pv( + self->constellation(t_tx, dt_transit)); + typename GPS_Ephemeris::constellation_res_t res = { + pv.position, pv.velocity, self->clock_error(t_tx), 0}; + return res; } #if defined(SWIGRUBY) %rename("consistent?") is_consistent; From 1c98afb881f6ea5c09592bc447fcdd89e619fba5 Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 2 Aug 2022 09:16:59 +0900 Subject: [PATCH 40/59] Add clock_error_dot() to GLONASS --- tool/navigation/GLONASS.h | 6 ++++++ tool/navigation/GLONASS_Solver.h | 3 +-- tool/swig/GPS.i | 2 +- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 44b5c784b..da61b83f4 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -1008,6 +1008,9 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} float_t clock_error(const float_t &t_depature_onboard) const { return calculate_clock_error(t_depature_onboard - Ephemeris::t_b); } + const float_t &clock_error_dot() const { + return Ephemeris::gamma_n; + } /** * Calculate absolute constellation(t) based on constellation(t_0). * t_0 is a time around t_b, and is used to calculate @@ -1189,6 +1192,9 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} float_t clock_error(const GPS_Time &t_tx) const{ return ephemeris().clock_error(t_tx); } + float_t clock_error_dot(const GPS_Time &t_tx) const{ + return ephemeris().clock_error_dot(); + } typename GPS_SpaceNode::SatelliteProperties::constellation_t constellation( const GPS_Time &t_tx, const float_t &dt_transit = 0) const { diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 7ee9c4598..31ca51ade 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -129,8 +129,7 @@ class GLONASS_SinglePositioning : public SolverBaseT { return sat(ptr).clock_error(t_tx); } static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) { - // Clock rate error is already taken into account in constellation() - return 0; + return sat(ptr).clock_error_dot(t_tx); } }; satellite_t res = { diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 42d2c52d2..c9c248d24 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -548,7 +548,7 @@ struct GLONASS_Ephemeris typename GPS_SpaceNode::SatelliteProperties::constellation_t pv( self->constellation(t_tx, dt_transit)); typename GPS_Ephemeris::constellation_res_t res = { - pv.position, pv.velocity, self->clock_error(t_tx), 0}; + pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot()}; return res; } #if defined(SWIGRUBY) From 55939fb05ee41d04390fce2669379a037a1bd2a0 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 24 Aug 2022 14:35:50 +0900 Subject: [PATCH 41/59] Fix to follow change of pointer separation in GPS_Solver_Base::satellite --- tool/navigation/GLONASS_Solver.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 31ca51ade..7cb74a623 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -133,7 +133,7 @@ class GLONASS_SinglePositioning : public SolverBaseT { } }; satellite_t res = { - &(it_sat->second), + &(it_sat->second), &(it_sat->second), impl_t::position, impl_t::velocity, impl_t::clock_error, impl_t::clock_error_dot}; return res; From 542875948d38b5e2dc0f111adcb758f45e59d955 Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 24 Aug 2022 14:37:23 +0900 Subject: [PATCH 42/59] Fix GLONASS to support RINEX clock for both INS_GPS and Ruby receiver --- tool/INS_GPS/GNSS_Receiver.h | 2 ++ tool/swig/GPS.i | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 612b3ca01..adb8c823c 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -193,6 +193,7 @@ struct GNSS_Receiver { // RINEX clock has higher priority to be applied than SP3 typename data_t::clk_t::count_t cnt2(data.clk.count()); if(cnt2.gps > 0){data.clk.push(gps.satellites, data_t::clk_t::SYSTEM_GPS);} + if(cnt2.glonass > 0){data.clk.push(glonass.satellites, data_t::clk_t::SYSTEM_GLONASS);} #endif } } solver_GNSS; @@ -440,6 +441,7 @@ struct GNSS_Receiver { std::cerr << "rinex_clk: " << entries << " items captured." << std::endl; typename data_t::clk_t::count_t cnt(data.clk.count()); if(cnt.gps > 0){std::cerr << "RINEX clock GPS satellites: " << cnt.gps << std::endl;} + if(cnt.glonass > 0){std::cerr << "RINEX clock GLONASS satellites: " << cnt.glonass << std::endl;} } solver_GNSS.update_ephemeris_source(data); return true; diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 32115ecc2..5b81c46b8 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -1547,8 +1547,8 @@ struct PushableData { case SYS_QZSS: break; case SYS_GLONASS: - return SP3_Product::push( - solver.glonass.solver.satellites, SP3_Product::SYSTEM_GLONASS); + return data.push( + solver.glonass.solver.satellites, DataT::SYSTEM_GLONASS); case SYS_GALILEO: case SYS_BEIDOU: default: From efd5d6d67c3e558cfeaadb60b2fbec0065cd2e70 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 2 Sep 2022 13:24:25 +0900 Subject: [PATCH 43/59] Fix procedure to calculate range sigma --- tool/navigation/GLONASS_Solver.h | 38 +++++++++++++++++++++----------- 1 file changed, 25 insertions(+), 13 deletions(-) diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 7cb74a623..9f3fd61d4 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -131,11 +131,15 @@ class GLONASS_SinglePositioning : public SolverBaseT { static float_t clock_error_dot(const void *ptr, const gps_time_t &t_tx) { return sat(ptr).clock_error_dot(t_tx); } + static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) { + return sat(ptr).ephemeris().F_T; + } }; satellite_t res = { - &(it_sat->second), &(it_sat->second), + &(it_sat->second), &(it_sat->second), // position, time impl_t::position, impl_t::velocity, - impl_t::clock_error, impl_t::clock_error_dot}; + impl_t::clock_error, impl_t::clock_error_dot, + &(it_sat->second), impl_t::range_sigma, NULL}; // error model return res; } satellites_t(const space_node_t &sn) @@ -292,21 +296,29 @@ class GLONASS_SinglePositioning : public SolverBaseT { res.range_residual += range_error.value[range_error_t::IONOSPHERIC]; } - // Setup weight - if(std::abs(res.range_residual) > _options.residual_mask){ - // If residual is too big, gently exclude it by decreasing its weight. - res.weight = 1E-8; - }else{ + // Setup range standard deviation, whose reciprocal square is used as weight + res.range_sigma = 1E+4; // sufficiently big value, 1E4 [m] + do{ + // If residual is too big, gently exclude it. + if(std::abs(res.range_residual) > _options.residual_mask){break;} float_t elv(relative_pos.elevation()); if(elv < _options.elevation_mask){ - res.weight = 0; // exclude it when elevation is less than threshold - }else{ - // elevation weight based on "GPS実用プログラミング" @see GPS_Solver.h - res.weight = std::pow(sin(elv)/0.8, 2); - if(res.weight < 1E-3){res.weight = 1E-3;} + res.range_sigma = 0; // exclude it when elevation is less than threshold + break; } - } + + res.range_sigma = sat.range_sigma(t_tx); + + /* elevation weight based on "GPS実用プログラミング" + * elevation[deg] : 90 53 45 30 15 10 5 + * sf_sigma(k) : 0.80 1.00 1.13 1.60 3.09 4.61 9.18 + * weight(k^-2) : 1.56 1.00 0.78 0.39 0.10 0.05 0.01 + */ + static const float_t max_sf(10); + static const float_t elv_limit(std::asin(0.8/max_sf)); // limit + res.range_sigma *= (elv > elv_limit) ? (0.8 / sin(elv)) : max_sf; + }while(false); res.range_corrected = range; From 0cc04ea92e58df48fefefa29791df7a74dbadbf8 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 2 Sep 2022 14:54:58 +0900 Subject: [PATCH 44/59] Refactor GLONASS F_T_value/index with GPS URA_meter/index --- tool/navigation/GLONASS.h | 15 ++++----------- tool/test/test_RINEX.cpp | 2 +- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index da61b83f4..9a4f415c2 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -814,11 +814,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} static const float_t F_T_table[15]; ///< @see Table 4.4 static const float_t F_T_value(const u8_t &F_T_){ - if(F_T_ >= (sizeof(F_T_table) / sizeof(F_T_table[0]))){ - return -1; // not used - }else{ - return F_T_table[F_T_]; - } + return GPS_SpaceNode::SatelliteProperties::Ephemeris::URA_meter( + F_T_, F_T_table); } static const uint_t P1_value(const u8_t &P1_){ @@ -1315,12 +1312,8 @@ typename GLONASS_SpaceNode::u8_t GLONASS_SpaceNode::SatellitePro if(F_T <= 0){ // invalid value return sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]); } - u8_t res(0); - while(res < (sizeof(raw_t::F_T_table) / sizeof(raw_t::F_T_table[0]))){ - if(F_T <= raw_t::F_T_table[res]){break;} - ++res; - } - return res; + return (u8_t)GPS_SpaceNode::SatelliteProperties::Ephemeris::URA_index( + F_T, raw_t::F_T_table); } template diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index 17b5d289c..266ecbbd9 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -396,7 +396,7 @@ BOOST_AUTO_TEST_CASE(nav_GLONASS_v3){ // default setting until ver.3.05 BOOST_CHECK_EQUAL(1, eph.M); // GLONASS-M BOOST_CHECK_EQUAL(60 * 60, eph.P1); // 1 hour - BOOST_CHECK_EQUAL(-1, eph.F_T); // invalid range accuracy + BOOST_CHECK_EQUAL(1024, eph.F_T); // invalid range accuracy, thus max(512) * 2 = 1024 will be returned. BOOST_CHECK_EQUAL(0, eph.delta_tau_n); } From 994aaee1b30b3d7be2a46aae0a92d0733908f6d9 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 21 Oct 2022 16:20:15 +0900 Subject: [PATCH 45/59] Fix typo in GLONASS code; almanac is correct --- tool/INS_GPS/GNSS_Data.h | 2 +- tool/navigation/GLONASS.h | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/tool/INS_GPS/GNSS_Data.h b/tool/INS_GPS/GNSS_Data.h index 96e340723..eeb2b16fe 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -218,7 +218,7 @@ struct GNSS_Data { eph.has_string = 0; } return true; - }else{ // non-immediate info. except for time info. (alnamac) + }else{ // non-immediate info. except for time info. (almanac) } return false; diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 9a4f415c2..0032eeb13 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -135,7 +135,7 @@ static s ## bits ## _t name(const InputT *buf){ \ convert_u( 8, 70, 5, n); convert_u( 8, 75, 2, M); }; - struct String5_Alnamac { + struct String5_Almanac { convert_u(16, 5, 11, NA); convert_s(32, 16, 32, tau_c); convert_u( 8, 49, 5, N_4); @@ -143,7 +143,7 @@ static s ## bits ## _t name(const InputT *buf){ \ convert_u( 8, 76, 1, l_n); }; - struct String6_Alnamac { // String 6; same as 8, 10, 12, 14 + struct String6_Almanac { // String 6; same as 8, 10, 12, 14 convert_u( 8, 5, 1, C_n); convert_u( 8, 6, 2, M_n); convert_u( 8, 8, 5, nA); @@ -152,7 +152,7 @@ static s ## bits ## _t name(const InputT *buf){ \ convert_s(32, 44, 18, delta_iA_n); convert_u(16, 62, 15, epsilonA_n); }; - struct String7_Alnamac { // String 7; same as 9, 11, 13, 15 + struct String7_Almanac { // String 7; same as 9, 11, 13, 15 convert_s(16, 5, 16, omegaA_n); convert_u(32, 21, 21, tA_lambda_n); convert_s(32, 42, 22, delta_TA_n); @@ -257,7 +257,7 @@ static s ## bits ## _t name(const InputT *buf){ \ bool l_n; #define fetch_item(name) name = BroadcastedMessage< \ InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \ - :: String5_Alnamac :: name (src) + :: String5_Almanac :: name (src) template void update_string5(const InputT *src){ fetch_item(tau_c); @@ -364,7 +364,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} uint_t E_n; // [days] uint_t P1; // [s] time interval of two adjacent t_b; 0 - 0, 1 - 30, 2 - 45, 3 - 60 mins bool P2; - //bool P3; // flag for alnamac; 1 - five, 0 - four + //bool P3; // flag for almanac; 1 - five, 0 - four bool P4; // flag for ephemeris; 1 - uploaded by the control segment bool l_n; // health flag; 0 - healthy, 1 - malfunction From edb4fe98c1d6a6b84281b11a59bd234cd2da433f Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 21 Oct 2022 14:55:29 +0900 Subject: [PATCH 46/59] Add (key)_set methods as provision of GLONASS raw data generation --- tool/navigation/GLONASS.h | 63 +++++++++++++++++++++++++++ tool/test/test_GLONASS.cpp | 87 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 150 insertions(+) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 0032eeb13..e8cea8442 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -46,6 +46,7 @@ #include "GPS.h" #include "algorithm/integral.h" +#include "util/bit_counter.h" template class GLONASS_SpaceNode { @@ -89,6 +90,10 @@ static u ## bits ## _t name(const InputT *buf){ \ return \ DataParser::template bits2num( \ buf, offset_bits, length); \ +} \ +static void name ## _set(InputT *dest, const u ## bits ## _t &src){ \ + DataParser::template num2bits( \ + dest, src, offset_bits, length, EffectiveBits, PaddingBits_MSB); \ } #define convert_s(bits, offset_bits, length, name) \ static s ## bits ## _t name(const InputT *buf){ \ @@ -97,9 +102,67 @@ static s ## bits ## _t name(const InputT *buf){ \ buf, offset_bits, length)); \ static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); /* MSB is sign bit */ \ return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \ +} \ +static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ + DataParser::template num2bits( \ + dest, *(u ## bits ## _t *)(&src), offset_bits, length, EffectiveBits, PaddingBits_MSB); \ } + convert_u( 8, 0, 1, idle); + static void idle_set(InputT *dest){idle_set(dest, 0);} convert_u( 8, 1, 4, m); convert_u( 8, 77, 8, KX); + static void KX_set(InputT *dest){ +#if 0 + mask_bits = 32 + mask_str = "0x%0#{mask_bits / 4}X" + [ + [9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43, + 45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84], + [9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44, + 45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84], + [10..12, 16..19, 23..26, 31..34, 38..41, 46..49, 54..57, 62..65, 69..72, 77..80, 85], + [13..19, 27..34, 42..49, 58..65, 73..80], + [20..34, 50..65, 81..85], + [35..65], + [66..85], + ].collect{|pat| + pat.collect{|idx| idx.to_a rescue idx}.flatten \ + .collect{|i| 85 - i}.sort.chunk{|i| i / mask_bits} \ + .collect{|quot, i_s| + mask = eval("0b#{i_s.inject("0" * mask_bits){|res, i| res[i % mask_bits] = '1'; res}}") + #mask = [mask_be].pack("N").unpack("L")[0] # endianess correction + [quot, mask] # endianess correction + }.inject([mask_str % [0]] * (77.0 / mask_bits).ceil){|res, (i, mask)| + res[i] = (mask_str % [mask]); res + } + }.transpose # => [ + ["0x55555AAA", "0x66666CCC", "0x87878F0F", "0x07F80FF0", "0xF8000FFF", "0x00000FFF", "0xFFFFF000"], + ["0xAAAAB555", "0xCCCCD999", "0x0F0F1E1E", "0x0FF01FE0", "0xF0001FFF", "0xFFFFE000", "0x00000000"], + ["0x6AD80000", "0xB3680000", "0x3C700000", "0x3F800000", "0xC0000000", "0x00000000", "0x00000000"] + ] +#endif + u8_t hamming(0); + static const struct { + uint_t idx, len; + u32_t mask[8]; + } prop[] = { + { 0, 32, {0x55555AAAu, 0x66666CCCu, 0x87878F0Fu, 0x07F80FF0u, 0xF8000FFFu, 0x00000FFFu, 0xFFFFF000u, ~0u}}, + {32, 32, {0xAAAAB555u, 0xCCCCD999u, 0x0F0F1E1Eu, 0x0FF01FE0u, 0xF0001FFFu, 0xFFFFE000u, 0, ~0u}}, + {64, 13, {0x6AD80000u >> 19, 0xB3680000u >> 19, 0x3C700000u >> 19, 0x3F800000u >> 19, 0xC0000000u >> 19, 0, 0, ~0u}}, + }; + for(std::size_t j(0); j < sizeof(prop) / sizeof(prop[0]); ++j){ + u8_t check_bit(1); + u32_t v(DataParser::template bits2num(dest, prop[j].idx, prop[j].len)); + for(int i(0); i < 8; ++i, check_bit <<= 1){ + if(BitCounter::count(v & prop[j].mask[i]) % 2 == 0){continue;} + hamming ^= check_bit; + } + } + if(BitCounter::count(hamming & 0x7F) % 2 == 1){ + hamming ^= 0x80; + } + KX_set(dest, hamming); + } struct String1 { convert_u( 8, 7, 2, P1); diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp index aeb923fec..e2d076f9a 100644 --- a/tool/test/test_GLONASS.cpp +++ b/tool/test/test_GLONASS.cpp @@ -44,6 +44,93 @@ struct Fixture { } }; +struct rtklib_t { + // https://github.com/tomojitakasu/RTKLIB/blob/03fb5a2da9ad4294a262b6ed8c4287dafdef4a74/src/rcvraw.c#L405 + static int test_glostr(const unsigned char *buff){ + static const unsigned char xor_8bit[256]={ /* xor of 8 bits */ + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0,1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1, + 1,0,0,1,0,1,1,0,0,1,1,0,1,0,0,1,0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0 + }; + static const unsigned char mask_hamming[][11]={ /* mask of hamming codes */ + {0x55,0x55,0x5A,0xAA,0xAA,0xAA,0xB5,0x55,0x6A,0xD8,0x08}, + {0x66,0x66,0x6C,0xCC,0xCC,0xCC,0xD9,0x99,0xB3,0x68,0x10}, + {0x87,0x87,0x8F,0x0F,0x0F,0x0F,0x1E,0x1E,0x3C,0x70,0x20}, + {0x07,0xF8,0x0F,0xF0,0x0F,0xF0,0x1F,0xE0,0x3F,0x80,0x40}, + {0xF8,0x00,0x0F,0xFF,0xF0,0x00,0x1F,0xFF,0xC0,0x00,0x80}, + {0x00,0x00,0x0F,0xFF,0xFF,0xFF,0xE0,0x00,0x00,0x01,0x00}, + {0xFF,0xFF,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0x00}, + {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xF8} + }; + unsigned char cs; /* checksum */ + int i,j,n=0; + for (i=0;i<8;i++) { + for (j=0,cs=0;j<11;j++) { + cs^=xor_8bit[buff[j]&mask_hamming[i][j]]; + } + if (cs) n++; + } + return n==0||(n==2&&cs); + } + template + static bool test_glostr(const boost::uint32_t (&buff)[N]){ + unsigned char temp[4 * N]; + for(std::size_t i(0), j(0); i < N; ++i){ + temp[j++] = (buff[i] >> 24) & 0xFF; + temp[j++] = (buff[i] >> 16) & 0xFF; + temp[j++] = (buff[i] >> 8) & 0xFF; + temp[j++] = buff[i] & 0xFF; + } + return test_glostr(temp) != 0; + } +}; + +BOOST_AUTO_TEST_CASE(parity_calculation){ + typedef boost::uint32_t u32_t; + static const u32_t packet[][4] = { + // UBX 20210531/log.ubx + {0x7839e8b8, 0x0d34c028, 0x97930800, 0x07a10002}, + {0x7839e8b8, 0x0d34c028, 0x979f1000, 0x07a10002}, + {0x08255533, 0x9cd061d6, 0x45355000, 0x07a10003}, + {0x082550fd, 0xd9f0f3cc, 0xf9511000, 0x07a10003}, + {0x08255521, 0x19e8af12, 0x4845b800, 0x07a10003}, + {0x08255400, 0x5c0136ff, 0x29fd9000, 0x07a10003}, + {0x08255407, 0x34d00255, 0x00ac6800, 0x07a10003}, + {0x08255023, 0x619865f3, 0xc02d0800, 0x07a10003}, + {0x10a500c4, 0x4111d648, 0xa8abe000, 0x07a10003}, + {0x10a504d0, 0xf090a084, 0x008eb000, 0x07a10003}, + {0x10a50016, 0xe050499e, 0x57b3c800, 0x07a10003}, + // UBX f9p_ppp_1224/rover.ubx + {0x654c07e3, 0xc5804cec, 0x2ed2f000, 0x0b4c0002}, + {0x654c09e3, 0xc5804cec, 0x2ed71000, 0x0b4c0002}, + {0x6f7c185c, 0xf774bf87, 0xc9e0c000, 0x0b4c0002}, + {0x755022ff, 0x49008c38, 0x36413000, 0x0b4c0002}, + {0x783990f3, 0xeb34bfe9, 0xcb95b800, 0x0b4c0002}, + {0x0801cc1b, 0x240c273a, 0x659d1000, 0x0b4c0003}, + {0x0801c954, 0xaf28622b, 0x0e39a800, 0x0b4c0003}, + {0x0801c8a3, 0xe6e470fb, 0xb1206800, 0x0b4c0003}, + {0x0801cc10, 0xb5342aa5, 0xc960f000, 0x0b4c0003}, + {0x0801cc12, 0xdfc80221, 0x76ec1000, 0x0b4c0003}, + {0x0801cd02, 0x905912b3, 0x78748000, 0x0b4c0003}, + }; + for(std::size_t i(0); i < sizeof(packet) / sizeof(packet[0]); ++i){ + bool res_rtklib(rtklib_t::test_glostr(packet[i])); + BOOST_CHECK(res_rtklib); + + typedef space_node_t::BroadcastedMessage msg_t; + u32_t copy[4]; + std::memcpy(copy, packet[i], sizeof(copy)); + msg_t::KX_set(copy, 0); + msg_t::KX_set(copy); + BOOST_CHECK_EQUAL(msg_t::KX(packet[i]), msg_t::KX(copy)); + } +} + BOOST_AUTO_TEST_CASE(ICD_CDMA_2016_Appendix_K){ space_node_t::TimeProperties::raw_t raw = {0}; raw.N_4 = 5; From 87a14f1db61e4df3a20c7aabab09225957a955f4 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 21 Oct 2022 15:58:44 +0900 Subject: [PATCH 47/59] add raw data dump() to GLONASS code and Ruby receiver --- tool/navigation/GLONASS.h | 72 ++++++++++++++++++++++++++++++++++++--- tool/swig/GPS.i | 25 ++++++++++++++ 2 files changed, 92 insertions(+), 5 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index e8cea8442..ecaf05907 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -113,6 +113,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ convert_u( 8, 77, 8, KX); static void KX_set(InputT *dest){ #if 0 + // mask generation with Ruby mask_bits = 32 mask_str = "0x%0#{mask_bits / 4}X" [ @@ -135,11 +136,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ }.inject([mask_str % [0]] * (77.0 / mask_bits).ceil){|res, (i, mask)| res[i] = (mask_str % [mask]); res } - }.transpose # => [ - ["0x55555AAA", "0x66666CCC", "0x87878F0F", "0x07F80FF0", "0xF8000FFF", "0x00000FFF", "0xFFFFF000"], - ["0xAAAAB555", "0xCCCCD999", "0x0F0F1E1E", "0x0FF01FE0", "0xF0001FFF", "0xFFFFE000", "0x00000000"], - ["0x6AD80000", "0xB3680000", "0x3C700000", "0x3F800000", "0xC0000000", "0x00000000", "0x00000000"] - ] + }.transpose #endif u8_t hamming(0); static const struct { @@ -330,6 +327,22 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ fetch_item(l_n); } #undef fetch_item + template + void dump(BufferT *dst){ + typedef BroadcastedMessage< + BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> + deparse_t; +#define dump_item(name) deparse_t::String5_Almanac:: name ## _set(dst, name) + dump_item(tau_c); + dump_item(tau_GPS); + dump_item(N_4); + dump_item(NA); + dump_item(l_n); +#undef dump_item + deparse_t::m_set(dst, 5); + deparse_t::idle_set(dst); + deparse_t::KX_set(dst); + } enum { SF_tau_c, SF_tau_GPS, @@ -861,6 +874,55 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} fetch_item(4, M); } #undef fetch_item + template + void dump(BufferT *dst, const unsigned int &str){ + typedef BroadcastedMessage< + BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> + deparse_t; +#define dump_item(num, name) deparse_t::String ## num :: name ## _set(dst, name) + switch(str){ + case 1: + dump_item(1, P1); + dump_item(1, t_k); + dump_item(1, xn_dot); + dump_item(1, xn_ddot); + dump_item(1, xn); + break; + case 2: { + dump_item(2, B_n); + dump_item(2, P2); + dump_item(2, t_b); + dump_item(2, yn_dot); + dump_item(2, yn_ddot); + dump_item(2, yn); + break; + } + case 3: + dump_item(3, P3); + dump_item(3, gamma_n); + dump_item(3, p); + dump_item(3, l_n); + dump_item(3, zn_dot); + dump_item(3, zn_ddot); + dump_item(3, zn); + break; + case 4: + dump_item(4, tau_n); + dump_item(4, delta_tau_n); + dump_item(4, E_n); + dump_item(4, P4); + dump_item(4, F_T); + dump_item(4, N_T); + dump_item(4, n); + dump_item(4, M); + default: + return; + } +#undef dump_item + deparse_t::m_set(dst, str); + deparse_t::idle_set(dst); + deparse_t::KX_set(dst); + } enum { SF_xn, SF_yn = SF_xn, SF_zn = SF_xn, diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index f8ecd497f..0029b5a1b 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -619,6 +619,31 @@ struct GLONASS_Ephemeris self->has_string = has_string; return updated; } + %apply unsigned int buf_brdc[ANY] { + unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3], + unsigned int buf_str4[3], unsigned int buf_str5[3]}; + /** + * Return broadcasted raw data of GLONASS ephemeris data. + * @param buf_str1 pointer to store raw data of string 1. + * 85bit length data (LSB 11 bits are padding) is stored in successive address of the pointer. + * @param buf_str2 pointer to store raw data of string 2. Its structue is same as str1. + * @param buf_str3 pointer to store raw data of string 3. Its structue is same as str1. + * @param buf_str4 pointer to store raw data of string 4. Its structue is same as str1. + * @param buf_str5 pointer to store raw data of string 5. Its structue is same as str1. + * @param t GPS time at broadcasting + */ + void dump( + unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3], + unsigned int buf_str4[3], unsigned int buf_str5[3], + const GPS_Time &t){ + typename GLONASS_Ephemeris::eph_t::raw_t raw; + raw = *self; + unsigned int *buf[4] = {buf_str1, buf_str2, buf_str3, buf_str4}; + for(int i(0); i < 4; ++i){ + raw.GLONASS_Ephemeris::Ephemeris::raw_t::dump<0, 0>(buf[i], i + 1); + } + raw.GLONASS_Ephemeris::TimeProperties::raw_t::dump<0, 0>(buf_str5); + } typename GPS_Ephemeris::constellation_res_t constellation( const GPS_Time &t_tx, const FloatT &dt_transit = 0) const { typename GPS_SpaceNode::SatelliteProperties::constellation_t pv( From 9dd7b0533e265958d7e4ad7b2e0aafff04963b3b Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 26 Oct 2022 16:16:11 +0900 Subject: [PATCH 48/59] Fix for precise GLONASS raw data conversion with std::floor --- tool/navigation/GLONASS.h | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index ecaf05907..94fea32eb 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -383,7 +383,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ raw_t &operator=(const TimeProperties &t){ #define CONVERT(TARGET) \ -{TARGET = (s32_t)((t.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);} +{TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);} CONVERT(tau_c); CONVERT(tau_GPS); std::div_t divmod(std::div(t.date.year - 1996, 4)); @@ -984,9 +984,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} return res; } raw_t &operator=(const Ephemeris &eph){ - // TODO: m? #define CONVERT(TARGET) \ -{TARGET = (s32_t)((eph.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);} +{TARGET = (s32_t)std::floor(eph.TARGET / sf[SF_ ## TARGET] + 0.5);} svid = eph.svid; { // t_k std::div_t minutes(div(eph.t_k, 60)); From f5477d9b6a0e0bf6f6d2540b82e6e3d208d48ef8 Mon Sep 17 00:00:00 2001 From: fenrir Date: Fri, 28 Oct 2022 22:39:21 +0900 Subject: [PATCH 49/59] Add GLONASS_Ephemeris#valid? in Ruby receiver --- tool/swig/GPS.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 0029b5a1b..796307873 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -531,6 +531,9 @@ struct GLONASS_Ephemeris bool is_consistent() const { return has_string == 0x1F; } + bool is_valid(const GPS_Time &t) const { + return is_consistent() && eph_t::is_valid(t); + } GLONASS_Ephemeris() : eph_t() { invalidate(); } @@ -654,6 +657,7 @@ struct GLONASS_Ephemeris } #if defined(SWIGRUBY) %rename("consistent?") is_consistent; + %rename("valid?") is_valid; %rename("in_range?") is_in_range; #endif bool is_in_range(const GPS_Time &t) const { From 86dafba8718ee00746d5070416519e37b7a006bc Mon Sep 17 00:00:00 2001 From: fenrir Date: Mon, 7 Nov 2022 23:18:32 +0900 Subject: [PATCH 50/59] Fix string 4 of GLONASS broadcast data dump() --- tool/navigation/GLONASS.h | 1 + 1 file changed, 1 insertion(+) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index 94fea32eb..dbc41ae2b 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -915,6 +915,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} dump_item(4, N_T); dump_item(4, n); dump_item(4, M); + break; default: return; } From 2ea62cb9f1747c51ff231f0de324bd31919f776f Mon Sep 17 00:00:00 2001 From: fenrir Date: Tue, 8 Nov 2022 16:17:23 +0900 Subject: [PATCH 51/59] Fix GLONASS broadcast data dump in negative values (sign bit handling) --- tool/navigation/GLONASS.h | 7 +++-- tool/test/test_GLONASS.cpp | 52 +++++++++++++++++++++++++++++++++++++- 2 files changed, 56 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index dbc41ae2b..bae3ec889 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -100,12 +100,15 @@ static s ## bits ## _t name(const InputT *buf){ \ u ## bits ## _t temp( \ DataParser::template bits2num( \ buf, offset_bits, length)); \ - static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); /* MSB is sign bit */ \ + /* MSB is sign bit (Not equal to two's complement?) */ \ + static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \ return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \ } \ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ + static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \ + u ## bits ## _t src2((src < 0) ? ((-src) | mask) : src); \ DataParser::template num2bits( \ - dest, *(u ## bits ## _t *)(&src), offset_bits, length, EffectiveBits, PaddingBits_MSB); \ + dest, src2, offset_bits, length, EffectiveBits, PaddingBits_MSB); \ } convert_u( 8, 0, 1, idle); static void idle_set(InputT *dest){idle_set(dest, 0);} diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp index e2d076f9a..2aab9a59c 100644 --- a/tool/test/test_GLONASS.cpp +++ b/tool/test/test_GLONASS.cpp @@ -90,7 +90,7 @@ struct rtklib_t { } }; -BOOST_AUTO_TEST_CASE(parity_calculation){ +BOOST_AUTO_TEST_CASE(dump_with_parity_check){ typedef boost::uint32_t u32_t; static const u32_t packet[][4] = { // UBX 20210531/log.ubx @@ -125,6 +125,56 @@ BOOST_AUTO_TEST_CASE(parity_calculation){ typedef space_node_t::BroadcastedMessage msg_t; u32_t copy[4]; std::memcpy(copy, packet[i], sizeof(copy)); +#define pingpong(str_num, key) { \ + u32_t copy2[4] = {0}; \ + msg_t::String ## str_num::key ## _set(copy2, msg_t::String ## str_num::key(copy)); \ + BOOST_REQUIRE_EQUAL(msg_t::String ## str_num::key(copy), msg_t::String ## str_num::key(copy2)); \ +} + pingpong(1, P1); + pingpong(1, t_k); + pingpong(1, xn_dot); + pingpong(1, xn_ddot); + pingpong(1, xn); + pingpong(2, B_n); + pingpong(2, P2); + pingpong(2, t_b); + pingpong(2, yn_dot); + pingpong(2, yn_ddot); + pingpong(2, yn); + pingpong(3, P3); + pingpong(3, gamma_n); + pingpong(3, p); + pingpong(3, l_n); + pingpong(3, zn_dot); + pingpong(3, zn_ddot); + pingpong(3, zn); + pingpong(4, tau_n); + pingpong(4, delta_tau_n); + pingpong(4, E_n); + pingpong(4, P4); + pingpong(4, F_T); + pingpong(4, N_T); + pingpong(4, n); + pingpong(4, M); + pingpong(5_Almanac, NA); + pingpong(5_Almanac, tau_c); + pingpong(5_Almanac, N_4); + pingpong(5_Almanac, tau_GPS); + pingpong(5_Almanac, l_n); + pingpong(6_Almanac, C_n); + pingpong(6_Almanac, M_n); + pingpong(6_Almanac, nA); + pingpong(6_Almanac, tauA_n); + pingpong(6_Almanac, lambdaA_n); + pingpong(6_Almanac, delta_iA_n); + pingpong(6_Almanac, epsilonA_n); + pingpong(7_Almanac, omegaA_n); + pingpong(7_Almanac, tA_lambda_n); + pingpong(7_Almanac, delta_TA_n); + pingpong(7_Almanac, delta_TA_dot_n); + pingpong(7_Almanac, HA_n); + pingpong(7_Almanac, l_n); +#undef pingpong msg_t::KX_set(copy, 0); msg_t::KX_set(copy); BOOST_CHECK_EQUAL(msg_t::KX(packet[i]), msg_t::KX(copy)); From 230942c0159a22b2091f967e6c30e6e8d1b342e6 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 19 Nov 2022 23:22:57 +0900 Subject: [PATCH 52/59] Make GLONASS_Solver ctor private --- tool/navigation/GLONASS_Solver.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 9f3fd61d4..22c208912 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -57,13 +57,15 @@ struct GLONASS_SinglePositioning_Options : public GPS_Solver_GeneralOptions > class GLONASS_SinglePositioning : public SolverBaseT { - private: - GLONASS_SinglePositioning &operator=(const GLONASS_SinglePositioning &); public: - typedef GLONASS_SinglePositioning self_t; + typedef GLONASS_SinglePositioning self_t; typedef SolverBaseT base_t; typedef SolverBaseT super_t; + private: + self_t &operator=(const self_t &); + GLONASS_SinglePositioning(const self_t &); + public: #if defined(__GNUC__) && (__GNUC__ < 5) #define inheritate_type(x) typedef typename base_t::x x; #else From c7c3e0ab0ec998031b1ba946f08ffd0c33edac2d Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 7 Jan 2023 21:18:15 +0900 Subject: [PATCH 53/59] Add getter/setter of GLONASS ephemeris date in SWIG GPS.i --- tool/swig/GPS.i | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index af7e205e2..3c6c333e9 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -586,6 +586,12 @@ struct GLONASS_Ephemeris MAKE_ACCESSOR(tau_c, FloatT); MAKE_ACCESSOR(tau_GPS, FloatT); + MAKE_ACCESSOR2(year, date.year, int); + MAKE_ACCESSOR2(day_of_year, date.day_of_year, int); + + void set_date(const unsigned int &N_4, const unsigned int &NA) { + self->date = GLONASS_SpaceNode::TimeProperties::raw_t::raw2date(N_4, NA); + } FloatT frequency_L1() const { return self->L1_frequency(); From c4349e6b793e20cfc7b1d222eb2f9a80c6570af3 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sat, 7 Jan 2023 21:40:19 +0900 Subject: [PATCH 54/59] Add GLONASS_Ephemeris#rehash resetting internal info in SWIG GPS.i --- tool/swig/GPS.i | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 3c6c333e9..943f8e9b4 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -557,6 +557,14 @@ struct GLONASS_Ephemeris raw = *this; has_string = 0x1F; } + GLONASS_Ephemeris &rehash(const int &deltaT = 0) { + typedef typename GLONASS_SpaceNode::SatelliteProperties prop_t; + return *this = GLONASS_Ephemeris(eph_t( + typename prop_t::Ephemeris_with_Time( + (typename prop_t::Ephemeris)(*this), + (typename GLONASS_SpaceNode::TimeProperties)(*this)), + deltaT)); + } }; %} %extend GLONASS_Ephemeris { From e148a6d89b6b8fe1da9cb9ab38d195293236d0fa Mon Sep 17 00:00:00 2001 From: fenrir Date: Wed, 11 Jan 2023 14:21:33 +0900 Subject: [PATCH 55/59] Fix GLONASS N_T handling Ephemeris calculation shold be based on N_T, which sometimes differs from Almanac N_A. N_T and N_A are currently assumed to belong to the same N_4 (no roll over). --- tool/navigation/GLONASS.h | 34 ++++++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 10 deletions(-) diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h index bae3ec889..a44aaed16 100644 --- a/tool/navigation/GLONASS.h +++ b/tool/navigation/GLONASS.h @@ -371,6 +371,18 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ } return res; } + static void date2raw(const date_t &src, u8_t *N_4_, u16_t *NA_){ + std::div_t divmod(std::div(src.year - 1996, 4)); + if(N_4_){*N_4_ = divmod.quot + 1;} + if(NA_){ + *NA_ = src.day_of_year + 1; + switch(divmod.rem){ + case 1: *NA_ += 366; break; + case 2: *NA_ += 731; break; + case 3: *NA_ += 1096; break; + } + } + } operator TimeProperties() const { TimeProperties res; @@ -389,14 +401,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \ {TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);} CONVERT(tau_c); CONVERT(tau_GPS); - std::div_t divmod(std::div(t.date.year - 1996, 4)); - N_4 = divmod.quot + 1; - NA = t.date.day_of_year + 1; - switch(divmod.rem){ - case 1: NA += 366; break; - case 2: NA += 731; break; - case 3: NA += 1096; break; - } + date2raw(t.date, &N_4, &NA); l_n = (t.l_n ? 1 : 0); #undef CONVERT return *this; @@ -1072,6 +1077,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} * 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.) */ struct Ephemeris_with_Time : public Ephemeris, TimeProperties { + typename TimeProperties::date_t t_b_date; typedef typename Ephemeris::constellation_t constellation_t; constellation_t xa_t_b; float_t sidereal_t_b_rad; @@ -1080,8 +1086,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} static const float_t time_step_max_per_one_integration; void calculate_additional() { + u8_t N_4; + u16_t NA; + TimeProperties::raw_t::date2raw(TimeProperties::date, &N_4, &NA); + // TODO detect large difference between NA and N_T caused by rolling over. + t_b_date = TimeProperties::raw_t::raw2date(N_4, this->N_T); sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg( - TimeProperties::date.c_tm(), + t_b_date.c_tm(), (float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI; constellation_t x_t_b = { {this->xn, this->yn, this->zn}, @@ -1101,11 +1112,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;} t_mt.tm_hour += 3; std::mktime(&t_mt); // renormalization this->date = TimeProperties::date_t::from_c_tm(t_mt); + u16_t N_T; + TimeProperties::raw_t::date2raw(this->date, NULL, &N_T); + this->N_T = N_T; this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec; calculate_additional(); } std::tm c_tm_utc() const { - std::tm t(TimeProperties::date.c_tm()); // set date on Moscow time + std::tm t(t_b_date.c_tm()); // set date on Moscow time (t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC std::mktime(&t); // renormalization return t; From adb5bce79d997e32de476c01982eee07588d9587 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sun, 22 Jan 2023 23:14:01 +0900 Subject: [PATCH 56/59] Add setter of GLONASS ephemeris date via std::tm in SWIG GPS.i --- tool/swig/GPS.i | 3 +++ 1 file changed, 3 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 9dc57181c..50a54b656 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -600,6 +600,9 @@ struct GLONASS_Ephemeris void set_date(const unsigned int &N_4, const unsigned int &NA) { self->date = GLONASS_SpaceNode::TimeProperties::raw_t::raw2date(N_4, NA); } + void set_date(const std::tm &t) { + self->date = GLONASS_SpaceNode::TimeProperties::date_t::from_c_tm(t); + } FloatT frequency_L1() const { return self->L1_frequency(); From 60083fa76be93841caf15ad95e78f68b4620d0d3 Mon Sep 17 00:00:00 2001 From: fenrir Date: Sun, 29 Jan 2023 22:43:18 +0900 Subject: [PATCH 57/59] Fix GLONASS_Ephemeris#in_range? in SWIG to just check time property --- tool/swig/GPS.i | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 50a54b656..70ec5cfa0 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -545,6 +545,10 @@ struct GLONASS_Ephemeris bool is_consistent() const { return has_string == 0x1F; } + bool is_in_range(const GPS_Time &t) const { + // "invalidate()" is used to make raw and converted data inconsistent. + return eph_t::is_valid(t); + } bool is_valid(const GPS_Time &t) const { return is_consistent() && eph_t::is_valid(t); } @@ -691,10 +695,6 @@ struct GLONASS_Ephemeris %rename("valid?") is_valid; %rename("in_range?") is_in_range; #endif - bool is_in_range(const GPS_Time &t) const { - // "invalidate()" is used to make raw and converted data inconsistent. - return self->is_valid(t); - } } %extend GLONASS_SpaceNode { From b1fe5b7e1cc49ab8b5f74b1bee16545014a96cc6 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 24 Oct 2024 23:31:43 +0900 Subject: [PATCH 58/59] Change GLONASS range/rate sigma treatment to be same as GPS --- tool/navigation/GLONASS_Solver.h | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h index 7bb5179d0..e5bc9b55a 100644 --- a/tool/navigation/GLONASS_Solver.h +++ b/tool/navigation/GLONASS_Solver.h @@ -134,7 +134,9 @@ class GLONASS_SinglePositioning : public SolverBaseT { return sat(ptr).clock_error_dot(t_tx); } static float_t range_sigma(const void *ptr, const gps_time_t &t_tx) { - return sat(ptr).ephemeris().F_T; + return std::sqrt( + std::pow(sat(ptr).ephemeris().F_T, 2) + + std::pow(4.5 / 1.96, 2)); // TODO change? (currently same as GPS) } }; satellite_t res = { @@ -229,7 +231,7 @@ class GLONASS_SinglePositioning : public SolverBaseT { * @param prn satellite number * @param measurement measurement (per satellite) containing pseudo range * @param receiver_error (temporal solution of) receiver clock error in meter - * @param time_arrival time when signal arrive at receiver + * @param time_arrival time when signal arrives at receiver * @param usr_pos (temporal solution of) user position * @param usr_vel (temporal solution of) user velocity * @return (relative_property_t) relative information @@ -325,11 +327,17 @@ class GLONASS_SinglePositioning : public SolverBaseT { res.range_corrected = range; xyz_t rel_vel(sat.velocity(t_tx, dt_transit) - usr_vel); // Calculate velocity - res.rate_relative_neg = res.los_neg[0] * rel_vel.x() + res.los_neg[1] * rel_vel.y() + res.los_neg[2] * rel_vel.z() + sat.clock_error_dot(t_tx) * c; + res.rate_sigma = sat.rate_sigma(time_arrival); + + if(_options.use_external_sigma){ + // Use standard deviation of pseudorange and/or its rate if they are provided by receiver + this->range_sigma(measurement, res.range_sigma); + this->rate_sigma(measurement, res.rate_sigma); + } return res; } From caa4be485945cb02131c8aa2ada24ecceb334417 Mon Sep 17 00:00:00 2001 From: fenrir Date: Thu, 28 Mar 2024 22:15:21 +0900 Subject: [PATCH 59/59] Add GLONASS F_T, P1, N_4, NA interface to SWIG wrapper --- tool/swig/GPS.i | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/tool/swig/GPS.i b/tool/swig/GPS.i index 05da03681..230366538 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -617,6 +617,21 @@ struct GLONASS_Ephemeris (typename GLONASS_SpaceNode::TimeProperties)(*this)), deltaT)); } + + unsigned char get_F_T_index() const { + return GLONASS_Ephemeris::F_T_index(); + } + unsigned char set_F_T_index(const unsigned char &idx) { + this->F_T = GLONASS_Ephemeris::raw_t::F_T_value(idx); + return get_F_T_index(); + } + unsigned char get_P1_index() const { + return GLONASS_Ephemeris::P1_index(); + } + unsigned char set_P1_index(const unsigned char &idx) { + this->P1 = GLONASS_Ephemeris::raw_t::P1_value(idx); + return get_P1_index(); + } }; %} %extend GLONASS_Ephemeris { @@ -649,12 +664,27 @@ struct GLONASS_Ephemeris MAKE_ACCESSOR2(year, date.year, int); MAKE_ACCESSOR2(day_of_year, date.day_of_year, int); + %rename(%str(F_T_index=)) set_F_T_index; + %rename(%str(F_T_index)) get_F_T_index; + %rename(%str(P1_index=)) set_P1_index; + %rename(%str(P1_index)) get_P1_index; + void set_date(const unsigned int &N_4, const unsigned int &NA) { self->date = GLONASS_SpaceNode::TimeProperties::raw_t::raw2date(N_4, NA); } void set_date(const std::tm &t) { self->date = GLONASS_SpaceNode::TimeProperties::date_t::from_c_tm(t); } + unsigned char N_4() const { + unsigned char res; + GLONASS_Ephemeris::TimeProperties::raw_t::date2raw(self->date, &res, NULL); + return res; + } + unsigned short NA() const { + unsigned short res; + GLONASS_Ephemeris::TimeProperties::raw_t::date2raw(self->date, NULL, &res); + return res; + } FloatT frequency_L1() const { return self->L1_frequency();