gps_pvt 0.8.5 → 0.9.0

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
@@ -371,6 +371,18 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
371
371
  }
372
372
  return res;
373
373
  }
374
+ static void date2raw(const date_t &src, u8_t *N_4_, u16_t *NA_){
375
+ std::div_t divmod(std::div(src.year - 1996, 4));
376
+ if(N_4_){*N_4_ = divmod.quot + 1;}
377
+ if(NA_){
378
+ *NA_ = src.day_of_year + 1;
379
+ switch(divmod.rem){
380
+ case 1: *NA_ += 366; break;
381
+ case 2: *NA_ += 731; break;
382
+ case 3: *NA_ += 1096; break;
383
+ }
384
+ }
385
+ }
374
386
 
375
387
  operator TimeProperties() const {
376
388
  TimeProperties res;
@@ -389,14 +401,7 @@ static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
389
401
  {TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);}
390
402
  CONVERT(tau_c);
391
403
  CONVERT(tau_GPS);
392
- std::div_t divmod(std::div(t.date.year - 1996, 4));
393
- N_4 = divmod.quot + 1;
394
- NA = t.date.day_of_year + 1;
395
- switch(divmod.rem){
396
- case 1: NA += 366; break;
397
- case 2: NA += 731; break;
398
- case 3: NA += 1096; break;
399
- }
404
+ date2raw(t.date, &N_4, &NA);
400
405
  l_n = (t.l_n ? 1 : 0);
401
406
  #undef CONVERT
402
407
  return *this;
@@ -1072,6 +1077,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
1072
1077
  * 3) t_GPS - t_GL = delta_T + tau_GPS (defined in ICD 4.5 Non-immediate info.)
1073
1078
  */
1074
1079
  struct Ephemeris_with_Time : public Ephemeris, TimeProperties {
1080
+ typename TimeProperties::date_t t_b_date;
1075
1081
  typedef typename Ephemeris::constellation_t constellation_t;
1076
1082
  constellation_t xa_t_b;
1077
1083
  float_t sidereal_t_b_rad;
@@ -1080,8 +1086,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
1080
1086
  static const float_t time_step_max_per_one_integration;
1081
1087
 
1082
1088
  void calculate_additional() {
1089
+ u8_t N_4;
1090
+ u16_t NA;
1091
+ TimeProperties::raw_t::date2raw(TimeProperties::date, &N_4, &NA);
1092
+ // TODO detect large difference between NA and N_T caused by rolling over.
1093
+ t_b_date = TimeProperties::raw_t::raw2date(N_4, this->N_T);
1083
1094
  sidereal_t_b_rad = TimeProperties::date_t::Greenwich_sidereal_time_deg(
1084
- TimeProperties::date.c_tm(),
1095
+ t_b_date.c_tm(),
1085
1096
  (float_t)(this->t_b) / (60 * 60) - 3) / 180 * M_PI;
1086
1097
  constellation_t x_t_b = {
1087
1098
  {this->xn, this->yn, this->zn},
@@ -1101,11 +1112,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
1101
1112
  t_mt.tm_hour += 3;
1102
1113
  std::mktime(&t_mt); // renormalization
1103
1114
  this->date = TimeProperties::date_t::from_c_tm(t_mt);
1115
+ u16_t N_T;
1116
+ TimeProperties::raw_t::date2raw(this->date, NULL, &N_T);
1117
+ this->N_T = N_T;
1104
1118
  this->t_b = (t_mt.tm_hour * 60 + t_mt.tm_min) * 60 + t_mt.tm_sec;
1105
1119
  calculate_additional();
1106
1120
  }
1107
1121
  std::tm c_tm_utc() const {
1108
- std::tm t(TimeProperties::date.c_tm()); // set date on Moscow time
1122
+ std::tm t(t_b_date.c_tm()); // set date on Moscow time
1109
1123
  (t.tm_sec = (int)(this->t_b)) -= 3 * 60 * 60; // add second on UTC
1110
1124
  std::mktime(&t); // renormalization
1111
1125
  return t;
@@ -658,10 +658,11 @@ protected:
658
658
  * @param x solution
659
659
  * @return WSSR scalar
660
660
  */
661
- float_t wssr(const matrix_t &x = least_square()) const {
661
+ float_t wssr(const matrix_t &x) const {
662
662
  matrix_t v(delta_r - G * x);
663
663
  return (v.transpose() * W * v)(0, 0);
664
664
  }
665
+ float_t wssr() const {return wssr(least_square());}
665
666
  /**
666
667
  * Calculate weighted square sum of residual (WSSR) based on least square solution
667
668
  * with solution coefficient matrix (S).
@@ -1809,7 +1809,7 @@ class RINEX_Writer {
1809
1809
  friend std::ostream &operator<<(std::ostream &out, const header_t &header){
1810
1810
  std::stringstream ss;
1811
1811
  ss << std::setfill(' ') << std::left;
1812
- for(header_t::const_iterator it(header.begin()), it_end(header.end());
1812
+ for(typename header_t::const_iterator it(header.begin()), it_end(header.end());
1813
1813
  it != it_end; ++it){
1814
1814
  ss << std::setw(60) << it->second.substr(0, 60)
1815
1815
  << std::setw(20) << it->first.substr(0, 20)
@@ -191,16 +191,19 @@ static std::string inspect_str(const VALUE &v){
191
191
  #endif
192
192
  }
193
193
 
194
- %define MAKE_ACCESSOR(name, type)
195
- %rename(%str(name ## =)) set_ ## name;
196
- type set_ ## name (const type &v) {
197
- return self->name = v;
194
+ %define MAKE_ACCESSOR2(func_name, target, type)
195
+ %rename(%str(func_name ## =)) set_ ## func_name;
196
+ type set_ ## func_name (const type &v) {
197
+ return self->target= v;
198
198
  }
199
- %rename(%str(name)) get_ ## name;
200
- const type &get_ ## name () const {
201
- return self->name;
199
+ %rename(%str(func_name)) get_ ## func_name;
200
+ const type &get_ ## func_name () const {
201
+ return self->target;
202
202
  }
203
203
  %enddef
204
+ %define MAKE_ACCESSOR(name, type)
205
+ MAKE_ACCESSOR2(name, name, type)
206
+ %enddef
204
207
 
205
208
  %define MAKE_VECTOR2ARRAY(type)
206
209
  %typemap(out) std::vector<type> {
@@ -663,6 +666,14 @@ struct GLONASS_Ephemeris
663
666
  raw = *this;
664
667
  has_string = 0x1F;
665
668
  }
669
+ GLONASS_Ephemeris &rehash(const int &deltaT = 0) {
670
+ typedef typename GLONASS_SpaceNode<FloatT>::SatelliteProperties prop_t;
671
+ return *this = GLONASS_Ephemeris(eph_t(
672
+ typename prop_t::Ephemeris_with_Time(
673
+ (typename prop_t::Ephemeris)(*this),
674
+ (typename GLONASS_SpaceNode<FloatT>::TimeProperties)(*this)),
675
+ deltaT));
676
+ }
666
677
  };
667
678
  %}
668
679
  %extend GLONASS_Ephemeris {
@@ -692,6 +703,15 @@ struct GLONASS_Ephemeris
692
703
 
693
704
  MAKE_ACCESSOR(tau_c, FloatT);
694
705
  MAKE_ACCESSOR(tau_GPS, FloatT);
706
+ MAKE_ACCESSOR2(year, date.year, int);
707
+ MAKE_ACCESSOR2(day_of_year, date.day_of_year, int);
708
+
709
+ void set_date(const unsigned int &N_4, const unsigned int &NA) {
710
+ self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::raw_t::raw2date(N_4, NA);
711
+ }
712
+ void set_date(const std::tm &t) {
713
+ self->date = GLONASS_SpaceNode<FloatT>::TimeProperties::date_t::from_c_tm(t);
714
+ }
695
715
 
696
716
  FloatT frequency_L1() const {
697
717
  return self->L1_frequency();
@@ -1149,19 +1169,8 @@ struct GPS_Measurement {
1149
1169
  }
1150
1170
 
1151
1171
  %extend GPS_SolverOptions_Common {
1152
- %define MAKE_ACCESSOR2(name, type)
1153
- %rename(%str(name ## =)) set_ ## name;
1154
- type set_ ## name (const type &v) {
1155
- return self->cast_general()->name = v;
1156
- }
1157
- %rename(%str(name)) get_ ## name;
1158
- const type &get_ ## name () const {
1159
- return self->cast_general()->name;
1160
- }
1161
- %enddef
1162
- MAKE_ACCESSOR2(elevation_mask, FloatT);
1163
- MAKE_ACCESSOR2(residual_mask, FloatT);
1164
- #undef MAKE_ACCESSOR2
1172
+ MAKE_ACCESSOR2(elevation_mask, cast_general()->elevation_mask, FloatT);
1173
+ MAKE_ACCESSOR2(residual_mask, cast_general()->residual_mask, FloatT);
1165
1174
  MAKE_VECTOR2ARRAY(int);
1166
1175
  %ignore cast_general;
1167
1176
  }
@@ -1321,15 +1330,15 @@ struct HookableSolver : public BaseT {
1321
1330
  HookableSolver<
1322
1331
  GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >,
1323
1332
  GPS_Solver<FloatT> >
1324
- ::HookableSolver<GPS_SpaceNode<FloatT> >(const GPS_SpaceNode<FloatT> &sn)
1333
+ ::HookableSolver/*<GPS_SpaceNode<FloatT> >*/(const GPS_SpaceNode<FloatT> &sn) // to avoid out-of-line constructor error
1325
1334
  : GPS_Solver_MultiFrequency<GPS_SinglePositioning<FloatT> >(sn), hook(NULL) {}
1326
1335
  template <> template <>
1327
1336
  HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
1328
- ::HookableSolver<SBAS_SpaceNode<FloatT> >(const SBAS_SpaceNode<FloatT> &sn)
1337
+ ::HookableSolver/*<SBAS_SpaceNode<FloatT> >*/(const SBAS_SpaceNode<FloatT> &sn)
1329
1338
  : SBAS_SinglePositioning<FloatT>(sn), hook(NULL) {}
1330
1339
  template <> template <>
1331
1340
  HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> >
1332
- ::HookableSolver<GLONASS_SpaceNode<FloatT> >(const GLONASS_SpaceNode<FloatT> &sn)
1341
+ ::HookableSolver/*<GLONASS_SpaceNode<FloatT> >*/(const GLONASS_SpaceNode<FloatT> &sn)
1333
1342
  : GLONASS_SinglePositioning<FloatT>(sn), hook(NULL) {}
1334
1343
  template <>
1335
1344
  GPS_Solver<FloatT>::base_t::relative_property_t
@@ -906,6 +906,8 @@ __RINEX_CLK_TEXT__
906
906
  }
907
907
  puts
908
908
  }
909
+ expect(solver.gps_options.elevation_mask).to eq(0)
910
+ expect(solver.gps_options.residual_mask).to be > 0
909
911
  end
910
912
 
911
913
  it 'can be modified through hooks' do
data/lib/gps_pvt/ntrip.rb CHANGED
@@ -88,8 +88,8 @@ class Ntrip < Net::HTTP
88
88
  }
89
89
  req
90
90
  end
91
- def get_source_table(header = {})
92
- Ntrip.parse_source_table(request(generate_request('/', header)).read_body)
91
+ def get_source_table(header = {}, &b)
92
+ (b || proc{|str| Ntrip.parse_source_table(str)}).call(request(generate_request('/', header)).read_body)
93
93
  end
94
94
  def get_data(mount_point, header = {}, &b)
95
95
  request(generate_request("/#{mount_point}", header)){|res|
@@ -129,10 +129,16 @@ OpenURI.class_eval{
129
129
  def OpenURI.open_ntrip(buf, target, proxy, options) # :nodoc:
130
130
  GPS_PVT::Ntrip.start(target.host, target.port){|ntrip|
131
131
  # get source table
132
- tbl = ntrip.get_source_table(options)
132
+ tbl = ntrip.get_source_table(options){|str| str}
133
+ if target.root? then
134
+ buf << tbl
135
+ buf.io.rewind
136
+ next
137
+ end
138
+ tbl = GPS_PVT::Ntrip::parse_source_table(tbl)
133
139
 
134
140
  # check mount point
135
- mnt_pt = target.path.sub(%r|^/|, '')
141
+ mnt_pt = target.mount_point
136
142
  prop = tbl.mount_points[mnt_pt]
137
143
  raise Net::ProtocolError::new("Mount point(#{mnt_pt}) not found") unless prop
138
144
 
@@ -154,10 +160,23 @@ OpenURI.class_eval{
154
160
  }
155
161
  module URI
156
162
  class Ntrip < HTTP
163
+ def root
164
+ res = self.clone
165
+ res.path = '/'
166
+ res
167
+ end
168
+ def root?; self.path == '/'; end
169
+ def mount_point
170
+ self.path.sub(%r|^/|, '')
171
+ end
172
+
157
173
  def buffer_open(buf, proxy, options)
158
174
  OpenURI.open_ntrip(buf, self, proxy, options)
159
175
  end
160
176
  include OpenURI::OpenRead
177
+ def read_source_table(options = {})
178
+ GPS_PVT::Ntrip::parse_source_table(self.root.read(options))
179
+ end
161
180
  end
162
181
  if respond_to?(:register_scheme) then
163
182
  register_scheme('NTRIP', Ntrip)
@@ -0,0 +1,110 @@
1
+ =begin
2
+ RTCM3 handler for receiver
3
+ =end
4
+
5
+ module GPS_PVT
6
+ class Receiver
7
+ def parse_rtcm3(src, opt = {}, &b)
8
+ $stderr.print "Reading RTCM3 stream (%s) "%[src]
9
+ require_relative '../rtcm3'
10
+ src_io = open(src)
11
+ rtcm3 = GPS_PVT::RTCM3::new(src_io)
12
+ ref_time = case (ref_time = opt[:ref_time])
13
+ when GPS::Time;
14
+ when Time
15
+ t_array = ref_time.utc.to_a[0..5].reverse
16
+ GPS::Time::new(t_array, GPS::Time::guess_leap_seconds(t_array))
17
+ when nil; GPS::Time::now
18
+ else; raise "reference time (#{ref_time}) should be GPS::Time or Time"
19
+ end
20
+ leap_sec = ref_time.leap_seconds
21
+ after_run = b || proc{|pvt| puts pvt.to_s if pvt}
22
+ t_meas, meas = [nil, GPS::Measurement::new]
23
+ dt_threshold = GPS::Time::Seconds_week / 2
24
+
25
+ while packet = rtcm3.read_packet
26
+ msg_num = packet.message_number
27
+ parsed = packet.parse
28
+ case msg_num
29
+ when 1019, 1044
30
+ params = parsed.params
31
+ if msg_num == 1044
32
+ params[:svid] += 192
33
+ params[:fit_interval] ||= 2 * 60 * 60
34
+ end
35
+ params[:WN] += ((ref_time.week - params[:WN]).to_f / 1024).round * 1024
36
+ eph = GPS::Ephemeris::new
37
+ params.each{|k, v| eph.send("#{k}=".to_sym, v)}
38
+ critical{
39
+ @solver.gps_space_node.register_ephemeris(eph.svid, eph)
40
+ }
41
+ when 1020
42
+ params = parsed.params
43
+ eph = GPS::Ephemeris_GLONASS::new
44
+ params[:F_T] ||= 10 # [m]
45
+ params.each{|k, v|
46
+ next if [:P3, :NA, :N_4].include?(k)
47
+ eph.send("#{k}=".to_sym, v)
48
+ }
49
+ proc{|date_src|
50
+ eph.set_date(*(date_src.all? \
51
+ ? date_src \
52
+ : [(ref_time + 3 * 60 * 60).c_tm(leap_sec)])) # UTC -> Moscow time
53
+ }.call([:N_4, :NA].collect{|k| params[k]})
54
+ eph.rehash(leap_sec)
55
+ critical{
56
+ @solver.glonass_space_node.register_ephemeris(eph.svid, eph)
57
+ }
58
+ when 1077, 1087, 1097, 1117
59
+ ranges = parsed.ranges
60
+ sig_list, svid_offset = case msg_num
61
+ when 1077 # GPS
62
+ t_meas ||= proc{ # update time of measurement
63
+ t_meas_sec = parsed[2][0] # DF004
64
+ dt = t_meas_sec - ref_time.seconds
65
+ GPS::Time::new(ref_time.week + if dt <= -dt_threshold then; 1
66
+ elsif dt >= dt_threshold then; -1
67
+ else; 0; end, t_meas_sec)
68
+ }.call
69
+ [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
70
+ 15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
71
+ 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 0]
72
+ when 1087 # GLONASS
73
+ proc{
74
+ utc = parsed[3][0] - 60 * 60 * 3 # DF034 UTC(SU)+3hr
75
+ delta = (t_meas.seconds - utc).to_i % (60 * 60 * 24)
76
+ leap_sec = (delta >= (60 * 60 * 12)) ? delta - (60 * 60 * 12) : delta
77
+ }.call if t_meas
78
+ [{2 => [:L1, GPS::SpaceNode_GLONASS.light_speed / GPS::SpaceNode_GLONASS.L1_frequency_base]}, 0x100]
79
+ when 1117 # QZSS
80
+ [{2 => [:L1, GPS::SpaceNode.L1_WaveLength],
81
+ 15 => [:L2CM, GPS::SpaceNode.L2_WaveLength],
82
+ 16 => [:L2CL, GPS::SpaceNode.L2_WaveLength]}, 192]
83
+ else; [{}, 0]
84
+ end
85
+ [:sat_sig, :pseudo_range, :phase_range, :phase_range_rate].collect{|k|
86
+ ranges[k]
87
+ }.transpose.each{|(svid, sig), pr, cpr, dr|
88
+ prefix, len = sig_list[sig]
89
+ next unless prefix
90
+ if svid_offset > 0 then
91
+ @solver.glonass_space_node.update_all_ephemeris(t_meas)
92
+ eph = @solver.glonass_space_node.ephemeris(svid)
93
+ end
94
+ svid += svid_offset
95
+ meas.add(svid, "#{prefix}_PSEUDORANGE".to_sym, pr) if pr
96
+ meas.add(svid, "#{prefix}_RANGE_RATE".to_sym, dr) if dr
97
+ meas.add(svid, "#{prefix}_CARRIER_PHASE".to_sym, cpr / len) if cpr
98
+ }
99
+ else
100
+ #p({msg_num => parsed})
101
+ end
102
+ if (1070..1229).include?(msg_num) &&
103
+ (!parsed.more_data? rescue (packet.decode([1], 24 + 54)[0] == 0)) then
104
+ after_run.call(run(meas, t_meas), [meas, ref_time = t_meas]) if t_meas
105
+ t_meas, meas = [nil, GPS::Measurement::new]
106
+ end
107
+ end
108
+ end
109
+ end
110
+ end
@@ -712,4 +712,5 @@ class Receiver
712
712
  end
713
713
  end
714
714
 
715
+ require_relative 'receiver/rtcm3'
715
716
  require_relative 'receiver/extension'