diff --git a/tool/INS_GPS.cpp b/tool/INS_GPS.cpp index 8966e8073..c9c26b290 100644 --- a/tool/INS_GPS.cpp +++ b/tool/INS_GPS.cpp @@ -209,6 +209,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" @@ -1991,6 +1992,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], @@ -2058,6 +2060,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_Data.h b/tool/INS_GPS/GNSS_Data.h index 87bec4255..a870667bd 100644 --- a/tool/INS_GPS/GNSS_Data.h +++ b/tool/INS_GPS/GNSS_Data.h @@ -42,6 +42,7 @@ #include "navigation/GPS.h" #include "navigation/SBAS.h" #include "navigation/QZSS.h" +#include "navigation/GLONASS.h" template struct GNSS_Data { @@ -55,6 +56,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; @@ -65,6 +67,8 @@ struct GNSS_Data { gps_t *gps; typedef SBAS_SpaceNode sbas_t; sbas_t *sbas; + 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 { @@ -85,7 +89,14 @@ struct GNSS_Data { typedef typename gps_t::Ionospheric_UTC_Parameters gps_iono_utc_t; - Loader() : gps(NULL), sbas(NULL) { + 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) {} + } glonass_ephemeris[24]; + + Loader() : gps(NULL), sbas(NULL), glonass(NULL) { for(unsigned int i(0); i < sizeof(gps_ephemeris) / sizeof(gps_ephemeris[0]); ++i){ gps_ephemeris[i].svid = i + 1; } @@ -93,6 +104,9 @@ struct GNSS_Data { qzss_ephemeris[i].svid = i + 193; qzss_ephemeris[i].is_qzss = true; } + for(unsigned int i(0); i < sizeof(glonass_ephemeris) / sizeof(glonass_ephemeris[0]); ++i){ + glonass_ephemeris[i].svid = i + 1; + } } /** @@ -195,11 +209,66 @@ struct GNSS_Data { return true; } + 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) + || (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 + // 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; // 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); + load(eph_converted); // register ephemeris + eph.has_string = 0; + } + return true; + }else{ // non-immediate info. except for time info. (almanac) + + } + return false; + } + bool load(const GNSS_Data &data){ switch(data.subframe.gnssID){ // TODO: other satellite systems case observer_t::gnss_svid_t::SBAS: return load_sbas(data); + case observer_t::gnss_svid_t::GLONASS: + return load_glonass(data); case observer_t::gnss_svid_t::QZSS: if(data.subframe.bytes != sizeof(data.subframe.buffer)){return false;} case observer_t::gnss_svid_t::GPS: diff --git a/tool/INS_GPS/GNSS_Receiver.h b/tool/INS_GPS/GNSS_Receiver.h index 52de05f6d..40cb97186 100644 --- a/tool/INS_GPS/GNSS_Receiver.h +++ b/tool/INS_GPS/GNSS_Receiver.h @@ -38,8 +38,10 @@ #include "navigation/GPS.h" #include "navigation/SBAS.h" +#include "navigation/GLONASS.h" #include "navigation/GPS_Solver.h" #include "navigation/SBAS_Solver.h" +#include "navigation/GLONASS_Solver.h" #include "navigation/RINEX.h" #include "navigation/INS_GPS2_Tightly.h" @@ -93,6 +95,9 @@ struct GNSS_Receiver { typedef SBAS_SpaceNode sbas_space_node_t; typedef SBAS_SinglePositioning sbas_solver_t; + typedef GLONASS_SpaceNode glonass_space_node_t; + typedef GLONASS_SinglePositioning glonass_solver_t; + struct system_t; struct data_t { @@ -104,6 +109,10 @@ struct GNSS_Receiver { sbas_space_node_t space_node; typename sbas_solver_t::options_t solver_options; } sbas; + 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; @@ -113,12 +122,13 @@ struct GNSS_Receiver { typedef typename RINEX_CLK::satellites_t clk_t; clk_t clk; #endif - data_t() : gps(), sbas(), out_rinex_nav(NULL) {} + data_t() : gps(), sbas(), glonass(), out_rinex_nav(NULL) {} ~data_t(){ if(out_rinex_nav){ typename RINEX_NAV_Writer::space_node_list_t list = {&gps.space_node}; list.sbas = &sbas.space_node; list.qzss = &gps.space_node; + list.glonass = &glonass.space_node; RINEX_NAV_Writer::write_all(*out_rinex_nav, list); } } @@ -128,6 +138,7 @@ struct GNSS_Receiver { typedef solver_base_t base_t; gps_solver_t gps; sbas_solver_t sbas; + glonass_solver_t glonass; struct measurement_items_t : public gps_solver_t::measurement_items_t { // TODO }; @@ -156,6 +167,7 @@ struct GNSS_Receiver { : base_t(), gps(rcv.data.gps.space_node), sbas(rcv.data.sbas.space_node), + glonass(rcv.data.glonass.space_node), ephemeris_proxy(gps) { #if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) // ionospheric and tropospheric correction methods @@ -166,9 +178,11 @@ struct GNSS_Receiver { tropospheric.push_back(&gps.tropospheric_simplified); gps.ionospheric_correction = sbas.ionospheric_correction + = glonass.ionospheric_correction = ionospheric; gps.tropospheric_correction = sbas.tropospheric_correction + = glonass.tropospheric_correction = tropospheric; #endif } @@ -179,6 +193,7 @@ struct GNSS_Receiver { case system_t::GPS: return gps; case system_t::SBAS: return sbas; case system_t::QZSS: return gps; + case system_t::GLONASS: return glonass; } return *this; } @@ -205,6 +220,8 @@ struct GNSS_Receiver { MAKE_ENTRY(gps.tropospheric_correction), MAKE_ENTRY(sbas.ionospheric_correction), MAKE_ENTRY(sbas.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){ @@ -227,6 +244,7 @@ struct GNSS_Receiver { if(cnt.gps > 0){data.sp3.push(ephemeris_proxy.gps, SP3_Product::SYSTEM_GPS);} if(cnt.sbas > 0){data.sp3.push(sbas.satellites, SP3_Product::SYSTEM_SBAS);} if(cnt.qzss > 0){data.sp3.push(ephemeris_proxy.qzss, SP3_Product::SYSTEM_QZSS);} + if(cnt.glonass > 0){data.sp3.push(glonass.satellites, SP3_Product::SYSTEM_GLONASS);} #endif #if !defined(BUILD_WITHOUT_RINEX_CLK) // RINEX clock has higher priority to be applied than SP3 @@ -234,6 +252,7 @@ struct GNSS_Receiver { if(cnt2.gps > 0){data.clk.push(ephemeris_proxy.gps, data_t::clk_t::SYSTEM_GPS);} if(cnt2.sbas > 0){data.clk.push(sbas.satellites, data_t::clk_t::SYSTEM_SBAS);} if(cnt2.qzss > 0){data.clk.push(ephemeris_proxy.qzss, data_t::clk_t::SYSTEM_QZSS);} + if(cnt2.glonass > 0){data.clk.push(glonass.satellites, data_t::clk_t::SYSTEM_GLONASS);} #endif } } solver_GNSS; @@ -254,6 +273,7 @@ struct GNSS_Receiver { void setup(typename GNSS_Data::Loader &loader) const { loader.gps = &const_cast(data.gps.space_node); loader.sbas = &const_cast(data.sbas.space_node); + loader.glonass = &const_cast(data.glonass.space_node); } const solver_base_t &solver() const { @@ -268,11 +288,13 @@ struct GNSS_Receiver { // Select most preferable ephemeris data.gps.space_node.update_all_ephemeris(t); data.sbas.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.sbas.update_options(data.sbas.solver_options); + solver_GNSS.glonass.update_options(data.glonass.solver_options); } struct system_t { @@ -396,6 +418,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 @@ -415,6 +439,7 @@ struct GNSS_Receiver { typename RINEX_NAV_Reader::space_node_list_t list = {&data.gps.space_node}; list.sbas = &data.sbas.space_node; list.qzss = &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; @@ -446,6 +471,7 @@ struct GNSS_Receiver { if(cnt.gps > 0){std::cerr << "SP3 GPS satellites: " << cnt.gps << std::endl;} if(cnt.sbas > 0){std::cerr << "SP3 SBAS satellites: " << cnt.sbas << std::endl;} if(cnt.qzss > 0){std::cerr << "SP3 QZSS satellites: " << cnt.qzss << std::endl;} + if(cnt.glonass > 0){std::cerr << "SP3 GLONASS satellites: " << cnt.glonass << std::endl;} } solver_GNSS.update_ephemeris_source(data); return true; @@ -483,6 +509,7 @@ struct GNSS_Receiver { if(cnt.gps > 0){std::cerr << "RINEX clock GPS satellites: " << cnt.gps << std::endl;} if(cnt.sbas > 0){std::cerr << "RINEX clock SBAS satellites: " << cnt.sbas << std::endl;} if(cnt.qzss > 0){std::cerr << "RINEX clock QZSS satellites: " << cnt.qzss << std::endl;} + if(cnt.glonass > 0){std::cerr << "RINEX clock GLONASS satellites: " << cnt.glonass << std::endl;} } solver_GNSS.update_ephemeris_source(data); return true; @@ -491,7 +518,8 @@ struct GNSS_Receiver { #define option_apply(expr) \ data.gps.solver_options. expr; \ -data.sbas.solver_options. expr +data.sbas.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)); @@ -519,6 +547,7 @@ data.sbas.solver_options. expr 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.sbas.ionospheric_correction.add(&solver_GNSS.gps.ionospheric_ntcm_gl); + solver_GNSS.glonass.ionospheric_correction.add(&solver_GNSS.gps.ionospheric_ntcm_gl); return true; } @@ -594,6 +623,11 @@ data.sbas.solver_options. expr ? data.sbas.solver_options.exclude_prn.set(without) : data.sbas.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; @@ -648,6 +682,7 @@ data.sbas.solver_options. expr #if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) << ',' << "SBAS_PRN(120-158)" << ',' << "QZSS_PRN(193-202)" + << ',' << "GLONASS_PRN(1-24)" #endif ; } @@ -667,6 +702,10 @@ data.sbas.solver_options. expr out << ',' << "range_residual(QZSS:" << i << ')' << ',' << "weight(QZSS:" << i << ')'; } + for(int i(1); i <= 24; ++i){ + out << ',' << "range_residual(GLONASS:" << i << ')' + << ',' << "weight(GLONASS:" << i << ')'; + } #endif return out; } @@ -693,6 +732,13 @@ data.sbas.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); @@ -746,6 +792,7 @@ data.sbas.solver_options. expr #if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) << ',' << mask_printer_t(src.used_satellite_mask, 120, 158) // SBAS << ',' << mask_printer_t(src.used_satellite_mask, 193, 202) // QZSS + << ',' << mask_printer_t(src.used_satellite_mask, 1, 24, system_t::GLONASS) // GLONASS #endif ; }else{ @@ -770,6 +817,7 @@ data.sbas.solver_options. expr #if !defined(BUILD_WITHOUT_GNSS_MULTI_CONSTELLATION) {120, 158}, // SBAS {193, 202}, // QZSS + {satellite_id_t(system_t::GLONASS, 1), satellite_id_t(system_t::GLONASS, 24)}, // GLONASS #endif }; unsigned int i_row(0); @@ -852,6 +900,12 @@ data.sbas.solver_options. expr << ',' << "azimuth(QZSS:" << i << ')' << ',' << "elevation(QZSS:" << i << ')'; } + 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; } @@ -959,6 +1013,15 @@ data.sbas.solver_options. expr for(int i(193); i <= 202; ++i){ out << ',' << p(i, cmd_qzss) << ',' << p.az_el(i); } + 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/NinjaScanLight_tools.sln b/tool/NinjaScanLight_tools.sln index 85de9ef95..df1369096 100644 --- a/tool/NinjaScanLight_tools.sln +++ b/tool/NinjaScanLight_tools.sln @@ -31,6 +31,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 @@ -122,6 +124,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/misc/receiver_debug.rb b/tool/misc/receiver_debug.rb index 56744552c..971955fa4 100644 --- a/tool/misc/receiver_debug.rb +++ b/tool/misc/receiver_debug.rb @@ -161,7 +161,7 @@ def initialize(options = {}) } @debug = {} @semaphore = Mutex::new - solver_opts = [:gps_options, :sbas_options].collect{|target| + solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target| @solver.send(target) } solver_opts.each{|opt| @@ -282,6 +282,11 @@ def v.to_b; !(self =~ /^(?:false|0|f|off)$/i); end prns.each{|prn| @solver.sbas_options.send(mode, prn)} elsif check_sys_svid.call(:QZSS, 193..202) then [svid || (193..202).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)} else raise "Unknown satellite: #{spec}" end @@ -408,12 +413,18 @@ def run(meas, t_meas, ref_pos = @base_station) } } - def register_ephemeris(t_meas, sys, prn, bcast_data) + def register_ephemeris(t_meas, sys, prn, bcast_data, *options) @eph_list ||= Hash[*((1..32).to_a + (193..202).to_a).collect{|prn| eph = GPS::Ephemeris::new eph.svid = prn [prn, eph] }.flatten(1)] + @eph_glonass_list ||= Hash[*(1..24).collect{|num| + eph = GPS::Ephemeris_GLONASS::new + eph.svid = num + [num, eph] + }.flatten(1)] + opt = options[0] || {} case sys when :GPS, :QZSS return unless eph = @eph_list[prn] @@ -443,6 +454,15 @@ def register_ephemeris(t_meas, sys, prn, bcast_data) $stderr.puts str } if @debug[:SBAS_IGP] end if t_meas + when :GLONASS + return 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) + return 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 end make_critical :register_ephemeris @@ -529,6 +549,11 @@ def parse_ubx(ubx_fname, &b) sigid = :L1 when :QZSS sigid = {0 => :L1, 5 => :L2CL, 4 => :L2CM}[sigid] + when :GLONASS + svid += 0x100 + sigid = {0 => :L1}[sigid] # TODO: to support {2 -> :L2} + meas.add(svid, :L1_FREQUENCY, + GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7)) else; next end next unless sigid @@ -572,10 +597,12 @@ def parse_ubx(ubx_fname, &b) }.call(packet.slice(6 + 2, 40).pack("C*").unpack("V*"))) 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]).pack("C*").unpack("V*")) + packet.slice(6 + 8, 4 * packet[6 + 4]).pack("C*").unpack("V*"), opt) end } $stderr.puts ", found packets are %s"%[ubx_kind.inspect] @@ -585,6 +612,7 @@ def parse_rinex_nav(fname) items = [ @solver.gps_space_node, @solver.sbas_space_node, + @solver.glonass_space_node, ].inject(0){|res, sn| loaded_items = critical{sn.send(:read, fname)} raise "Format error! (Not RINEX) #{fname}" if loaded_items < 0 @@ -597,6 +625,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 @@ -620,12 +649,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{|(sys, prn), v| case sys when 'G', ' ' when 'S'; prn += 100 when 'J'; prn += 192 + 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] @@ -730,7 +778,7 @@ def attach_rinex_clk(fname) files.collect!{|fname, ftype| raise "File not found: #{fname}" unless File::exist?(fname) ftype ||= case fname - when /\.\d{2}[nhq]$/; :rinex_nav + when /\.\d{2}[nhqg]$/; :rinex_nav when /\.\d{2}o$/; :rinex_obs when /\.ubx$/; :ubx when /\.sp3$/; :sp3 diff --git a/tool/navigation/GLONASS.h b/tool/navigation/GLONASS.h new file mode 100644 index 000000000..a44aaed16 --- /dev/null +++ b/tool/navigation/GLONASS.h @@ -0,0 +1,1466 @@ +/* + * 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 + +#include "GPS.h" +#include "algorithm/integral.h" +#include "util/bit_counter.h" + +template +class GLONASS_SpaceNode { + public: + typedef FloatT float_t; + + static const float_t light_speed; + static const float_t L1_frequency_base; + static const float_t L1_frequency_gap; + static const float_t L2_frequency_base; + static const float_t L2_frequency_gap; + + 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; + + 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 + 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); \ +} \ +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){ \ + u ## bits ## _t temp( \ + DataParser::template bits2num( \ + buf, offset_bits, length)); \ + /* 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, src2, 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 generation with Ruby + 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 +#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); + 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_Almanac { + 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_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); + 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_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); + 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; ///< 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 { + 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 { + 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 < (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;} + 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 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) + 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; // health flag; 0 - healthy, 1 - malfunction + + 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_Almanac :: 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 + 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, + 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; + } + 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; +#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)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);} + CONVERT(tau_c); + CONVERT(tau_GPS); + date2raw(t.date, &N_4, &NA); + l_n = (t.l_n ? 1 : 0); +#undef CONVERT + 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 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] + 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] + 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 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 + + float_t L1_frequency() const { + return GLONASS_SpaceNode::L1_frequency(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 + 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; + } + 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, .11) + static const float_t omega_E; + constellation_t abs_corrdinate(const float_t &sidereal_time_in_rad){ + // @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), + 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 + 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; + } + 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. + // 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; +#endif + } + }; + + 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; + } + 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 arg_t { + float_t xi, eta, zeta, r; + } m, s; + 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 lunar_solar_perturbations_t setup( + const constatnt_t &C, const var_t &var){ + + 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)); + + { // 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(-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(-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); + + float_t zeta_11(xi_ast * sepsilon + zeta_ast * cepsilon); + float_t zeta_12(zeta_11 * sepsilon - eta_ast * cepsilon); + + // (ICD4) float_t Gamma_m(Gamma_o + Gamma_1 * T); + float_t sGamma(std::sin(var.Gamma_m)), cGamma(std::cos(var.Gamma_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); + 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); + } + + { // 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)); + + // (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) + + 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 { + 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) {} + /** + * @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; + 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(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), + rho2(a_e * a_e / r2); + constellation_t res = { + {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; + } + }; + + 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_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); + } + }; + + 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; + + // 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 + 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); + break; + 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, + 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 + + static const float_t F_T_value(const u8_t &F_T_){ + return GPS_SpaceNode::SatelliteProperties::Ephemeris::URA_meter( + F_T_, F_T_table); + } + + 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) \ +{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 + 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; + + res.F_T = F_T_value(F_T); + res.n = n; + CONVERT(delta_tau_n); + res.E_n = E_n; + res.P1 = P1_value(P1); + 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){ +#define CONVERT(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)); + 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); // t_k is just a reference time + 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; + } + }; + + /** + * + * Relationship of time systems + * 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 { + 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; + typename Ephemeris::differential_t eq_of_motion; + + 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( + 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}, + {this->xn_dot, this->yn_dot, this->zn_dot}, + }; + 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) + : 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); + 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(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; + } + struct raw_t : public Ephemeris::raw_t, TimeProperties::raw_t { + operator Ephemeris_with_Time() const { + return Ephemeris_with_Time((Ephemeris)(*this), (TimeProperties)(*this)); + } + 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(const float_t &delta_t) const { + return -TimeProperties::tau_c - Ephemeris::tau_n + Ephemeris::gamma_n * delta_t; + } + /** + * @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_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 + * @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 + * 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 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 &delta_t, + const constellation_t &xa_t_0, const float_t &t_0_from_t_b) const { + + 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). + * @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( + const float_t &delta_t, const float_t &dt_transit = 0) const { + + constellation_t res( + constellation_abs(delta_t, xa_t_b, float_t(0))); + + return res.rel_corrdinate( + sidereal_t_b_rad + (constellation_t::omega_E * (delta_t + dt_transit))); // transform from abs to PZ-90.02 + } + /** + * @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_depature_glonass, const float_t &dt_transit = 0) const { + return calculate_constellation( + t_depature_glonass - Ephemeris::t_b, dt_transit); // measure in UTC + 3hr scale + } + }; + + 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 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_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; + } + 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(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_depature_gps, const float_t &dt_transit = 0) const { + return this->calculate_constellation(t_depature_gps - t_b_gps, dt_transit); + } + }; + }; + + struct Satellite : public SatelliteProperties { + public: + typedef typename SatelliteProperties::Ephemeris_with_GPS_Time eph_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_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_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_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_depature_from_t_0 -= delta_t_updated; + } + + 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))); + } + }; + + 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_cached_t(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_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 { + return (typename GPS_SpaceNode::SatelliteProperties::constellation_t)( + eph_history.current().constellation(t_tx, dt_transit)); + } + + 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_tx, const float_t &dt_transit = 0) const { + return constellation(t_tx, dt_transit).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); + } + } + 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 +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::light_speed + = 299792458; // [m/s] + +template +const typename GLONASS_SpaceNode::float_t GLONASS_SpaceNode::L1_frequency_base + = 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_base + = 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] + +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)) \ + : (((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 [s], unit is described as [day] in ICD, however, it may be wrong. +}; + +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_n + POWER_2(-30), // delta_tau_n +}; + +#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]); + } + return (u8_t)GPS_SpaceNode::SatelliteProperties::Ephemeris::URA_index( + F_T, raw_t::F_T_table); +} + +template +const typename GLONASS_SpaceNode::float_t + GLONASS_SpaceNode::SatelliteProperties::Ephemeris_with_Time + ::time_step_max_per_one_integration = 60; + +#endif /* __GLONASS_H__ */ diff --git a/tool/navigation/GLONASS_Solver.h b/tool/navigation/GLONASS_Solver.h new file mode 100644 index 000000000..e5bc9b55a --- /dev/null +++ b/tool/navigation/GLONASS_Solver.h @@ -0,0 +1,351 @@ +/** + * @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.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 { + public: + 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 +#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); + 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; + + 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_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_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_tx) { + return sat(ptr).clock_error(t_tx); + } + 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 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 = { + &(it_sat->second), &(it_sat->second), // position, time + impl_t::position, impl_t::velocity, + 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) + : 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(){} + + 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 + * + * @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 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 + */ + 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 + + 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 / c) * c); + }else{ + range += range_error.value[range_error_t::SATELLITE_CLOCK]; + } + + // Calculate satellite position + 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.distance(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){ + 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]; + } + + // 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.range_sigma = 0; // exclude it when elevation is less than threshold + break; + } + + res.range_sigma = sat.range_sigma(t_tx); + + /* elevation weight based on "GPSpvO~O" + * 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; + + 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; + } +}; + +template +const typename SolverBaseT::measurement_item_set_t + GLONASS_SinglePositioning::L1OF + = SolverBaseT::L1CA; + +#endif /* __GLONASS_SOLVER_H__ */ diff --git a/tool/navigation/RINEX.h b/tool/navigation/RINEX.h index 110b349f5..39221bf45 100644 --- a/tool/navigation/RINEX.h +++ b/tool/navigation/RINEX.h @@ -55,6 +55,7 @@ #include "util/text_helper.h" #include "GPS.h" #include "SBAS.h" +#include "GLONASS.h" template class RINEX_Reader { @@ -444,6 +445,72 @@ struct RINEX_NAV { return eph; } }; + 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) + + // 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()), + 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), + 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}; + 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; + 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); + } + }; }; template @@ -455,6 +522,7 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { typedef typename RINEX_NAV::space_node_t space_node_t; typedef typename RINEX_NAV::message_t message_t; typedef typename RINEX_NAV::message_sbas_t message_sbas_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]; @@ -471,11 +539,18 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { static const typename super_t::convert_item_t eph2_sbas_v2[4], eph2_sbas_v3[4]; static const typename super_t::convert_item_t eph3_sbas_v2[4], eph3_sbas_v3[4]; + 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]; + 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; message_t msg; message_sbas_t msg_sbas; - + message_glonass_t msg_glonass; + void seek_next_v2_gps() { char buf[256]; @@ -512,9 +587,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; } @@ -608,6 +701,31 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { sys_of_msg = super_t::version_type_t::SYS_QZSS; } + 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 < ((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); + + 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; + case 4: super_t::convert(eph4_glonass_v305, 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++){ @@ -627,7 +745,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_qzss(buf); return; // QZSS case 'C': seek_next_v3_not_implemented(buf, 8); return; // Beido case 'S': seek_next_v3_sbas(buf); return; // SBAS @@ -735,10 +853,79 @@ 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, 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 { + t_corr_glonass.flags = 0; + super_t::header_t::const_iterator it; + + 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((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 t_corr_glonass.flags > 0; + } + + bool extract_t_corr_glonass_v3(t_corr_glonass_t &t_corr_glonass) const { + iono_utc_t iono_utc; + 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; + + 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){ + 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; + } + } + } + + 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; + t_corr_glonass.flags |= t_corr_glonass_t::LEAP_SEC; + } + + return t_corr_glonass.flags > 0; + } + struct space_node_list_t { space_node_t *gps; SBAS_SpaceNode *sbas; space_node_t *qzss; + GLONASS_SpaceNode *glonass; }; static int read_all(std::istream &in, space_node_list_t &space_nodes = {0}){ @@ -755,6 +942,12 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { && (reader.version_type.version >= 302)){ reader.extract_iono_utc_v3(*space_nodes.qzss); } + 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){ @@ -777,6 +970,20 @@ class RINEX_NAV_Reader : public RINEX_Reader<> { 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; + eph0.tau_GPS = t_corr_glonass.tau_GPS; + 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; + } default: break; } } @@ -1321,6 +1528,86 @@ const typename RINEX_NAV_Reader::convert_item_t RINEX_NAV_Reader }; +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::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]), @@ -1383,6 +1670,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), @@ -1610,6 +1905,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { typedef typename RINEX_NAV::space_node_t space_node_t; typedef typename RINEX_NAV::message_t message_t; typedef typename RINEX_NAV::message_sbas_t message_sbas_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; @@ -1742,6 +2038,45 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { dest << 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 < ((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; + } + break; + } + dest << buf.str(); + return *this; + } public: void set_version( @@ -1755,6 +2090,7 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { const space_node_t *gps; const SBAS_SpaceNode *sbas; const space_node_t *qzss; + const GLONASS_SpaceNode *glonass; }; int write_all( const space_node_list_t &space_nodes, @@ -1801,6 +2137,53 @@ 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.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) && (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 + -latest.tau_c, + }; + 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()) && (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)){ + // 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); } @@ -1825,6 +2208,10 @@ class RINEX_NAV_Writer : public RINEX_Writer<> { w << message_sbas_t(eph); counter++; } + void operator()(const typename message_glonass_t::eph_t &eph) { + w << message_glonass_t(eph); + counter++; + } } functor = {*this, res, false, false}; if(space_nodes.gps){ functor.gps = true; @@ -1858,6 +2245,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/swig/GPS.i b/tool/swig/GPS.i index 4bad345a5..3d04644bf 100644 --- a/tool/swig/GPS.i +++ b/tool/swig/GPS.i @@ -20,6 +20,7 @@ #include "navigation/GPS.h" #include "navigation/SBAS.h" #include "navigation/QZSS.h" +#include "navigation/GLONASS.h" #include "navigation/RINEX.h" #include "navigation/RINEX_Clock.h" #include "navigation/SP3.h" @@ -30,6 +31,7 @@ #include "navigation/GPS_Solver_RAIM.h" #include "navigation/GPS_Solver_MultiFrequency.h" #include "navigation/SBAS_Solver.h" +#include "navigation/GLONASS_Solver.h" #if defined(__cplusplus) && (__cplusplus < 201103L) namespace std { @@ -700,6 +702,226 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode::SatelliteProperties::Ephe %include navigation/SBAS.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; + } + 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); + } + 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; + } + 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)); + } + + 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 { + 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); + 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(); + }; + FloatT frequency_L2() const { + return self->L2_frequency(); + }; + 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; + } + %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( + self->constellation(t_tx, dt_transit)); + typename GPS_Ephemeris::constellation_res_t res = { + pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot()}; + return res; + } +#if defined(SWIGRUBY) + %rename("consistent?") is_consistent; + %rename("valid?") is_valid; + %rename("in_range?") is_in_range; +#endif +} + +%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; @@ -1091,6 +1313,25 @@ struct SBAS_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 @@ -1157,6 +1398,8 @@ struct HookableSolver : public BaseT { %ignore gps; %ignore sbas_t; %ignore sbas; + %ignore glonass_t; + %ignore glonass; %ignore select; %ignore relative_property; %ignore select_satellite; @@ -1180,6 +1423,10 @@ struct HookableSolver : public BaseT { HookableSolver, GPS_Solver > ::HookableSolver/* >*/(const SBAS_SpaceNode &sn) : SBAS_SinglePositioning(sn), hook(NULL) {} + template <> template <> + HookableSolver, GPS_Solver > + ::HookableSolver/* >*/(const GLONASS_SpaceNode &sn) + : GLONASS_SinglePositioning(sn), hook(NULL) {} template <> GPS_Solver::base_t::relative_property_t GPS_Solver::relative_property( @@ -1343,6 +1590,8 @@ struct HookableSolver : public BaseT { ID2SYM(rb_intern("gps_tropospheric")), ID2SYM(rb_intern("sbas_ionospheric")), ID2SYM(rb_intern("sbas_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"))); @@ -1512,6 +1761,12 @@ struct GPS_Solver HookableSolver, GPS_Solver > solver; sbas_t() : space_node(), options(), solver(space_node) {} } sbas; + struct glonass_t { + GLONASS_SpaceNode space_node; + GLONASS_SolverOptions options; + HookableSolver, GPS_Solver > solver; + glonass_t() : space_node(), options(), solver(space_node) {} + } glonass; SWIG_Object hooks; typedef std::vector > user_correctors_t; user_correctors_t user_correctors; @@ -1527,7 +1782,7 @@ struct GPS_Solver } #endif GPS_Solver() : super_t(), - gps(), sbas(), + gps(), sbas(), glonass(), hooks(), user_correctors() { #ifdef SWIGRUBY hooks = rb_hash_new(); @@ -1539,22 +1794,28 @@ struct GPS_Solver tropospheric.push_back(&gps.solver.tropospheric_simplified); gps.solver.ionospheric_correction = sbas.solver.ionospheric_correction + = glonass.solver.ionospheric_correction = ionospheric; gps.solver.tropospheric_correction = sbas.solver.tropospheric_correction + = glonass.solver.tropospheric_correction = tropospheric; gps.solver.hook = this; sbas.solver.hook = this; + glonass.solver.hook = this; } GPS_SpaceNode &gps_space_node() {return gps.space_node;} GPS_SolverOptions &gps_options() {return gps.options;} SBAS_SpaceNode &sbas_space_node() {return sbas.space_node;} SBAS_SolverOptions &sbas_options() {return sbas.options;} + GLONASS_SpaceNode &glonass_space_node() {return glonass.space_node;} + GLONASS_SolverOptions &glonass_options() {return glonass.options;} const base_t &select( const typename base_t::prn_t &prn) const { if(prn > 0 && prn <= 32){return gps.solver;} if(prn >= 120 && prn <= 158){return sbas.solver;} if(prn > 192 && prn <= 202){return gps.solver;} + if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;} return *this; } // proxy of virtual functions @@ -1597,6 +1858,8 @@ struct GPS_Solver const_cast(gps).solver.update_options(gps.options); const_cast(sbas).space_node.update_all_ephemeris(receiver_time); const_cast(sbas).solver.update_options(sbas.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 @@ -1611,6 +1874,8 @@ struct GPS_Solver &gps.solver.tropospheric_correction, &sbas.solver.ionospheric_correction, &sbas.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{ @@ -1775,6 +2040,8 @@ struct PushableData { return data.push( solver.gps.ephemeris_proxy.qzss, DataT::SYSTEM_QZSS); case SYS_GLONASS: + return data.push( + solver.glonass.solver.satellites, DataT::SYSTEM_GLONASS); case SYS_GALILEO: case SYS_BEIDOU: default: @@ -1788,7 +2055,7 @@ struct PushableData { SYS_GPS, SYS_SBAS, SYS_QZSS, - //SYS_GLONASS, + SYS_GLONASS, //SYS_GALILEO, //SYS_BEIDOU, }; @@ -1914,6 +2181,10 @@ struct RINEX_Clock : public RINEX_CLK::satellites_t, PushableData { %template(SpaceNode_SBAS) SBAS_SpaceNode; %template(SolverOptions_SBAS) SBAS_SolverOptions; +%template(SpaceNode_GLONASS) GLONASS_SpaceNode; +%template(Ephemeris_GLONASS) GLONASS_Ephemeris; +%template(SolverOptions_GLONASS) GLONASS_SolverOptions; + %template(RINEX_Observation) RINEX_Observation; %template(SP3) SP3; %template(RINEX_Clock) RINEX_Clock; diff --git a/tool/test/test_GLONASS.cpp b/tool/test/test_GLONASS.cpp new file mode 100644 index 000000000..2aab9a59c --- /dev/null +++ b/tool/test/test_GLONASS.cpp @@ -0,0 +1,287 @@ +#if defined(_MSC_VER) +#define _USE_MATH_DEFINES +#endif + +#include +#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; + } +}; + +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(dump_with_parity_check){ + 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)); +#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)); + } +} + +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_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_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() 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 パッケージの復元] を使用します。詳細については、http://go.microsoft.com/fwlink/?LinkID=322105 を参照してください。見つからないファイルは {0} です。 + + + + + + + \ No newline at end of file diff --git a/tool/test/test_RINEX.cpp b/tool/test/test_RINEX.cpp index 6e51cd6e4..1ec91811b 100644 --- a/tool/test/test_RINEX.cpp +++ b/tool/test/test_RINEX.cpp @@ -1,3 +1,6 @@ +#if defined(_MSC_VER) +#define _USE_MATH_DEFINES +#endif #include #include #include @@ -5,6 +8,7 @@ #include "navigation/RINEX.h" #include "navigation/RINEX_Clock.h" #include "navigation/GPS.h" +#include "navigation/GLONASS.h" #include #include @@ -22,6 +26,7 @@ using boost::format; typedef double fnum_t; typedef GPS_SpaceNode gps_t; typedef SBAS_SpaceNode sbas_t; +typedef GLONASS_SpaceNode glonass_t; BOOST_AUTO_TEST_SUITE(RINEX) @@ -498,6 +503,249 @@ BOOST_AUTO_TEST_CASE(nav_QZSS_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); + + glonass_t glonass; + { + std::stringbuf sbuf(src); + std::istream in(&sbuf); + reader_t::space_node_list_t space_nodes = {NULL}; + 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, 12); + 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 = {NULL}; + 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, 14); + 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); + + // 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(1024, eph.F_T); // invalid range accuracy, thus max(512) * 2 = 1024 will be returned. + BOOST_CHECK_EQUAL(0, eph.delta_tau_n); + } + + 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"; +#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; + writer.write_all(space_nodes, 304); + dist = ss.str(); + } + + //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"