gps_pvt 0.3.3 → 0.4.0

Sign up to get free protection for your applications and to get access to all the features.
@@ -20,12 +20,14 @@
20
20
  #include "navigation/GPS.h"
21
21
  #include "navigation/SBAS.h"
22
22
  #include "navigation/QZSS.h"
23
+ #include "navigation/GLONASS.h"
23
24
  #include "navigation/RINEX.h"
24
25
 
25
26
  #include "navigation/GPS_Solver_Base.h"
26
27
  #include "navigation/GPS_Solver.h"
27
28
  #include "navigation/GPS_Solver_RAIM.h"
28
29
  #include "navigation/SBAS_Solver.h"
30
+ #include "navigation/GLONASS_Solver.h"
29
31
 
30
32
  #if defined(__cplusplus) && (__cplusplus < 201103L)
31
33
  namespace std {
@@ -506,6 +508,160 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
506
508
 
507
509
  %include navigation/SBAS.h
508
510
 
511
+ %inline %{
512
+ template <class FloatT>
513
+ struct GLONASS_Ephemeris
514
+ : public GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time {
515
+ typedef typename GLONASS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris_with_GPS_Time eph_t;
516
+ unsigned int super_frame, has_string;
517
+ typename eph_t::raw_t raw;
518
+ void invalidate() {
519
+ super_frame = 0;
520
+ has_string = 0;
521
+ }
522
+ bool is_consistent() const {
523
+ return has_string == 0x1F;
524
+ }
525
+ GLONASS_Ephemeris() : eph_t() {
526
+ invalidate();
527
+ }
528
+ GLONASS_Ephemeris(const eph_t &eph)
529
+ : eph_t(eph),
530
+ super_frame(0), has_string(0), raw() {
531
+ raw = *this;
532
+ has_string = 0x1F;
533
+ }
534
+ };
535
+ %}
536
+ %extend GLONASS_Ephemeris {
537
+ MAKE_ACCESSOR(svid, unsigned int);
538
+
539
+ MAKE_ACCESSOR(freq_ch, int); // frequency channel to be configured
540
+ MAKE_ACCESSOR(t_k, unsigned int);
541
+ MAKE_ACCESSOR(t_b, unsigned int);
542
+ MAKE_ACCESSOR(M, unsigned int);
543
+ MAKE_ACCESSOR(gamma_n, FloatT);
544
+ MAKE_ACCESSOR(tau_n, FloatT);
545
+
546
+ MAKE_ACCESSOR(xn, FloatT); MAKE_ACCESSOR(xn_dot, FloatT); MAKE_ACCESSOR(xn_ddot, FloatT);
547
+ MAKE_ACCESSOR(yn, FloatT); MAKE_ACCESSOR(yn_dot, FloatT); MAKE_ACCESSOR(yn_ddot, FloatT);
548
+ MAKE_ACCESSOR(zn, FloatT); MAKE_ACCESSOR(zn_dot, FloatT); MAKE_ACCESSOR(zn_ddot, FloatT);
549
+
550
+ MAKE_ACCESSOR(B_n, unsigned int);
551
+ MAKE_ACCESSOR(p, unsigned int);
552
+ MAKE_ACCESSOR(N_T, unsigned int);
553
+ MAKE_ACCESSOR(F_T, FloatT);
554
+ MAKE_ACCESSOR(n, unsigned int);
555
+ MAKE_ACCESSOR(delta_tau_n, FloatT);
556
+ MAKE_ACCESSOR(E_n, unsigned int);
557
+ MAKE_ACCESSOR(P1, unsigned int);
558
+ MAKE_ACCESSOR(P2, bool);
559
+ MAKE_ACCESSOR(P4, bool);
560
+
561
+ MAKE_ACCESSOR(tau_c, FloatT);
562
+ MAKE_ACCESSOR(tau_GPS, FloatT);
563
+
564
+ FloatT frequency_L1() const {
565
+ return self->L1_frequency();
566
+ };
567
+ FloatT frequency_L2() const {
568
+ return self->L2_frequency();
569
+ };
570
+ GPS_Time<FloatT> base_time() const {
571
+ return self->base_time();
572
+ }
573
+
574
+ //MAKE_ACCESSOR(l_n, bool); // exists in both Ephemeris and Time_Properties
575
+
576
+ MAKE_ARRAY_INPUT(const unsigned int, buf, SWIG_AsVal(unsigned int));
577
+ bool parse(const unsigned int buf[4], const unsigned int &leap_seconds = 0){
578
+ typedef typename GLONASS_SpaceNode<FloatT>
579
+ ::template BroadcastedMessage<unsigned int> parser_t;
580
+ unsigned int super_frame(buf[3] >> 16), frame(buf[3] & 0xF), string_no(parser_t::m(buf));
581
+ unsigned int has_string(self->has_string);
582
+ if((has_string > 0) && (self->super_frame != super_frame)){
583
+ has_string = 0; // clean up
584
+ }
585
+ self->super_frame = super_frame;
586
+ has_string |= (0x1 << (string_no - 1));
587
+ switch(string_no){
588
+ case 1: self->raw.template update_string1<0, 0>(buf); break;
589
+ case 2: self->raw.template update_string2<0, 0>(buf); break;
590
+ case 3: self->raw.template update_string3<0, 0>(buf); break;
591
+ case 4: self->raw.template update_string4<0, 0>(buf); break;
592
+ case 5: {
593
+ self->raw.template update_string5<0, 0>(buf);
594
+ if(frame == 4){
595
+ // TODO: require special care for 50th frame? @see Table 4.9 note (4)
596
+ }
597
+ break;
598
+ }
599
+ }
600
+ bool updated(false);
601
+ if((has_string == 0x1F) && (has_string != self->has_string)){
602
+ updated = true;
603
+ // All ephemeris and time info. in the same super frame has been acquired,
604
+ // and this block is called once per one same super frame.
605
+ // Ephemeris_with_Time::raw_t =(cast)=> Ephemeris_with_Time => Ephemeris_with_GPS_Time
606
+ static_cast<GLONASS_Ephemeris<FloatT>::eph_t &>(*self)
607
+ = GLONASS_Ephemeris<FloatT>::eph_t(self->raw);
608
+ self->t_b_gps += leap_seconds;
609
+ }
610
+ self->has_string = has_string;
611
+ return updated;
612
+ }
613
+ FloatT clock_error(
614
+ const GPS_Time<FloatT> &t_arrival, const FloatT &pseudo_range = 0) const {
615
+ return self->clock_error(t_arrival, pseudo_range);
616
+ }
617
+ %typemap(in,numinputs=0) System_XYZ<FloatT, WGS84> & (System_XYZ<FloatT, WGS84> temp) %{
618
+ $1 = &temp;
619
+ %}
620
+ %typemap(argout) System_XYZ<FloatT, WGS84> & {
621
+ %append_output(SWIG_NewPointerObj((new $*1_ltype(*$1)), $1_descriptor, SWIG_POINTER_OWN));
622
+ }
623
+ void constellation(
624
+ System_XYZ<FloatT, WGS84> &position, System_XYZ<FloatT, WGS84> &velocity,
625
+ const GPS_Time<FloatT> &t, const FloatT &pseudo_range = 0) const {
626
+ typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t res(
627
+ self->constellation(t, pseudo_range));
628
+ position = res.position;
629
+ velocity = res.velocity;
630
+ }
631
+ #if defined(SWIGRUBY)
632
+ %rename("consistent?") is_consistent;
633
+ %rename("in_range?") is_in_range;
634
+ #endif
635
+ bool is_in_range(const GPS_Time<FloatT> &t) const {
636
+ // "invalidate()" is used to make raw and converted data inconsistent.
637
+ return self->is_valid(t);
638
+ }
639
+ }
640
+
641
+ %extend GLONASS_SpaceNode {
642
+ %fragment(SWIG_Traits_frag(FloatT));
643
+ %ignore satellites() const;
644
+ %ignore satellite(const int &);
645
+ %ignore latest_ephemeris() const;
646
+ void register_ephemeris(
647
+ const int &prn, const GLONASS_Ephemeris<FloatT> &eph,
648
+ const int &priority_delta = 1){
649
+ self->satellite(prn).register_ephemeris(eph, priority_delta);
650
+ }
651
+ GLONASS_Ephemeris<FloatT> ephemeris(const int &prn) const {
652
+ return GLONASS_Ephemeris<FloatT>(
653
+ %const_cast(self, GLONASS_SpaceNode<FloatT> *)->satellite(prn).ephemeris());
654
+ }
655
+ int read(const char *fname) {
656
+ std::fstream fin(fname, std::ios::in | std::ios::binary);
657
+ typename RINEX_NAV_Reader<FloatT>::space_node_list_t list = {NULL};
658
+ list.glonass = self;
659
+ return RINEX_NAV_Reader<FloatT>::read_all(fin, list);
660
+ }
661
+ }
662
+
663
+ %include navigation/GLONASS.h
664
+
509
665
  %extend GPS_User_PVT {
510
666
  %ignore solver_t;
511
667
  %ignore base_t;
@@ -597,8 +753,9 @@ struct GPS_User_PVT
597
753
  Matrix<FloatT, Array2D_Dense<FloatT> > G_enu() const {
598
754
  return proxy_t::linear_solver_t::rotate_G(base_t::G, base_t::user_position.ecef2enu());
599
755
  }
600
- typename proxy_t::linear_solver_t linear_solver() const {
601
- return typename proxy_t::linear_solver_t(base_t::G, base_t::W, base_t::delta_r);
756
+ typename proxy_t::linear_solver_t::partial_t linear_solver() const {
757
+ return typename proxy_t::linear_solver_t(base_t::G, base_t::W, base_t::delta_r)
758
+ .partial(used_satellites());
602
759
  }
603
760
  Matrix<FloatT, Array2D_Dense<FloatT> > C() const {
604
761
  return linear_solver().C();
@@ -611,14 +768,26 @@ struct GPS_User_PVT
611
768
  linear_solver().least_square(res);
612
769
  return res;
613
770
  }
771
+ Matrix<FloatT, Array2D_Dense<FloatT> > S_enu(
772
+ const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
773
+ return proxy_t::linear_solver_t::rotate_S(s, base_t::user_position.ecef2enu());
774
+ }
614
775
  Matrix<FloatT, Array2D_Dense<FloatT> > S_enu() const {
615
- return proxy_t::linear_solver_t::rotate_S(S(), base_t::user_position.ecef2enu());
776
+ return S_enu(S());
777
+ }
778
+ Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV(
779
+ const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
780
+ return linear_solver().slope_HV(s);
616
781
  }
617
782
  Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV() const {
618
- return linear_solver().slope_HV(S());
783
+ return slope_HV(S());
784
+ }
785
+ Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV_enu(
786
+ const Matrix<FloatT, Array2D_Dense<FloatT> > &s) const {
787
+ return linear_solver().slope_HV(s, base_t::user_position.ecef2enu());
619
788
  }
620
789
  Matrix<FloatT, Array2D_Dense<FloatT> > slope_HV_enu() const {
621
- return linear_solver().slope_HV(S(), base_t::user_position.ecef2enu());
790
+ return slope_HV_enu(S());
622
791
  }
623
792
 
624
793
  void fd(const typename base_t::detection_t **out) const {*out = &(base_t::FD);}
@@ -884,6 +1053,25 @@ struct SBAS_SolverOptions
884
1053
  };
885
1054
  %}
886
1055
 
1056
+ %extend GLONASS_SolverOptions {
1057
+ %ignore base_t;
1058
+ %ignore cast_general;
1059
+ MAKE_VECTOR2ARRAY(int);
1060
+ }
1061
+ %inline %{
1062
+ template <class FloatT>
1063
+ struct GLONASS_SolverOptions
1064
+ : public GLONASS_SinglePositioning<FloatT>::options_t,
1065
+ GPS_SolverOptions_Common<FloatT> {
1066
+ typedef typename GLONASS_SinglePositioning<FloatT>::options_t base_t;
1067
+ void exclude(const int &prn){base_t::exclude_prn.set(prn);}
1068
+ void include(const int &prn){base_t::exclude_prn.reset(prn);}
1069
+ std::vector<int> excluded() const {return base_t::exclude_prn.excluded();}
1070
+ GPS_Solver_GeneralOptions<FloatT> *cast_general(){return this;}
1071
+ const GPS_Solver_GeneralOptions<FloatT> *cast_general() const {return this;}
1072
+ };
1073
+ %}
1074
+
887
1075
  %header {
888
1076
  template <class FloatT>
889
1077
  struct GPS_RangeCorrector
@@ -912,6 +1100,8 @@ struct GPS_RangeCorrector
912
1100
  %ignore gps;
913
1101
  %ignore sbas_t;
914
1102
  %ignore sbas;
1103
+ %ignore glonass_t;
1104
+ %ignore glonass;
915
1105
  %ignore select_solver;
916
1106
  %ignore relative_property;
917
1107
  %ignore satellite_position;
@@ -1103,6 +1293,8 @@ struct GPS_RangeCorrector
1103
1293
  ID2SYM(rb_intern("gps_tropospheric")),
1104
1294
  ID2SYM(rb_intern("sbas_ionospheric")),
1105
1295
  ID2SYM(rb_intern("sbas_tropospheric")),
1296
+ ID2SYM(rb_intern("glonass_ionospheric")),
1297
+ ID2SYM(rb_intern("glonass_tropospheric")),
1106
1298
  };
1107
1299
  static const VALUE k_opt(ID2SYM(rb_intern("options")));
1108
1300
  static const VALUE k_f_10_7(ID2SYM(rb_intern("f_10_7")));
@@ -1224,6 +1416,12 @@ struct GPS_Solver
1224
1416
  SBAS_SinglePositioning<FloatT> solver;
1225
1417
  sbas_t() : space_node(), options(), solver(space_node) {}
1226
1418
  } sbas;
1419
+ struct glonass_t {
1420
+ GLONASS_SpaceNode<FloatT> space_node;
1421
+ GLONASS_SolverOptions<FloatT> options;
1422
+ GLONASS_SinglePositioning<FloatT> solver;
1423
+ glonass_t() : space_node(), options(), solver(space_node) {}
1424
+ } glonass;
1227
1425
  SWIG_Object hooks;
1228
1426
  typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
1229
1427
  user_correctors_t user_correctors;
@@ -1239,7 +1437,7 @@ struct GPS_Solver
1239
1437
  }
1240
1438
  #endif
1241
1439
  GPS_Solver() : super_t(),
1242
- gps(), sbas(),
1440
+ gps(), sbas(), glonass(),
1243
1441
  hooks(), user_correctors() {
1244
1442
  #ifdef SWIGRUBY
1245
1443
  hooks = rb_hash_new();
@@ -1251,20 +1449,25 @@ struct GPS_Solver
1251
1449
  tropospheric.push_back(&gps.solver.tropospheric_simplified);
1252
1450
  gps.solver.ionospheric_correction
1253
1451
  = sbas.solver.ionospheric_correction
1452
+ = glonass.solver.ionospheric_correction
1254
1453
  = ionospheric;
1255
1454
  gps.solver.tropospheric_correction
1256
1455
  = sbas.solver.tropospheric_correction
1456
+ = glonass.solver.tropospheric_correction
1257
1457
  = tropospheric;
1258
1458
  }
1259
1459
  GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
1260
1460
  GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
1261
1461
  SBAS_SpaceNode<FloatT> &sbas_space_node() {return sbas.space_node;}
1262
1462
  SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
1463
+ GLONASS_SpaceNode<FloatT> &glonass_space_node() {return glonass.space_node;}
1464
+ GLONASS_SolverOptions<FloatT> &glonass_options() {return glonass.options;}
1263
1465
  const base_t &select_solver(
1264
1466
  const typename base_t::prn_t &prn) const {
1265
1467
  if(prn > 0 && prn <= 32){return gps.solver;}
1266
1468
  if(prn >= 120 && prn <= 158){return sbas.solver;}
1267
1469
  if(prn > 192 && prn <= 202){return gps.solver;}
1470
+ if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;}
1268
1471
  // call order: base_t::solve => this returned by select()
1269
1472
  // => relative_property() => select_solver()
1270
1473
  // For not supported satellite, call loop prevention is required.
@@ -1292,6 +1495,8 @@ struct GPS_Solver
1292
1495
  const_cast<gps_t &>(gps).solver.update_options(gps.options);
1293
1496
  const_cast<sbas_t &>(sbas).space_node.update_all_ephemeris(receiver_time);
1294
1497
  const_cast<sbas_t &>(sbas).solver.update_options(sbas.options);
1498
+ const_cast<glonass_t &>(glonass).space_node.update_all_ephemeris(receiver_time);
1499
+ const_cast<glonass_t &>(glonass).solver.update_options(glonass.options);
1295
1500
  return super_t::solve().user_pvt(measurement.items, receiver_time);
1296
1501
  }
1297
1502
  typedef
@@ -1306,6 +1511,8 @@ struct GPS_Solver
1306
1511
  &gps.solver.tropospheric_correction,
1307
1512
  &sbas.solver.ionospheric_correction,
1308
1513
  &sbas.solver.tropospheric_correction,
1514
+ &glonass.solver.ionospheric_correction,
1515
+ &glonass.solver.tropospheric_correction,
1309
1516
  };
1310
1517
  for(std::size_t i(0); i < sizeof(root) / sizeof(root[0]); ++i){
1311
1518
  do{
@@ -1482,6 +1689,10 @@ struct RINEX_Observation {};
1482
1689
  %template(SpaceNode_SBAS) SBAS_SpaceNode<type>;
1483
1690
  %template(SolverOptions_SBAS) SBAS_SolverOptions<type>;
1484
1691
 
1692
+ %template(SpaceNode_GLONASS) GLONASS_SpaceNode<type>;
1693
+ %template(Ephemeris_GLONASS) GLONASS_Ephemeris<type>;
1694
+ %template(SolverOptions_GLONASS) GLONASS_SolverOptions<type>;
1695
+
1485
1696
  %template(RINEX_Observation) RINEX_Observation<type>;
1486
1697
  %enddef
1487
1698
 
@@ -153,7 +153,7 @@ class Receiver
153
153
  rel_prop
154
154
  }
155
155
  @debug = {}
156
- solver_opts = [:gps_options, :sbas_options].collect{|target|
156
+ solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target|
157
157
  @solver.send(target)
158
158
  }
159
159
  solver_opts.each{|opt|
@@ -268,6 +268,11 @@ class Receiver
268
268
  prns.each{|prn| @solver.sbas_options.send(mode, prn)}
269
269
  elsif check_sys_svid.call(:QZSS, 193..202) then
270
270
  [svid || (193..202).to_a].flatten.each{|prn| @solver.gps_options.send(mode, prn)}
271
+ elsif check_sys_svid.call(:GLONASS, 1..24, 0x100) then
272
+ prns = [svid || (1..24).to_a].flatten.collect{|i| (i & 0xFF) + 0x100}
273
+ labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"}
274
+ update_output.call(:GLONASS, prns, labels)
275
+ prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)}
271
276
  else
272
277
  raise "Unknown satellite: #{spec}"
273
278
  end
@@ -344,22 +349,23 @@ class Receiver
344
349
  [[:@azimuth, az], [:@elevation, el]].each{|k, values|
345
350
  self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
346
351
  }
352
+ mat_S = self.S
347
353
  [:@slopeH, :@slopeV] \
348
- .zip((self.fd ? self.slope_HV_enu.to_a.transpose : [nil, nil])) \
354
+ .zip((self.fd ? self.slope_HV_enu(mat_S).to_a.transpose : [nil, nil])) \
349
355
  .each{|k, values|
350
356
  self.instance_variable_set(k,
351
357
  Hash[*(values ? sats.zip(values).flatten(1) : [])])
352
358
  }
359
+ # If a design matrix G has columns larger than 4,
360
+ # other states excluding position and time are estimated.
361
+ @other_state = self.position_solved? \
362
+ ? (mat_S * self.delta_r.partial(self.used_satellites, 1, 0, 0)).transpose.to_a[0][4..-1] \
363
+ : []
353
364
  instance_variable_get(target)
354
365
  }
355
- [:azimuth, :elevation, :slopeH, :slopeV].each{|k|
366
+ [:azimuth, :elevation, :slopeH, :slopeV, :other_state].each{|k|
356
367
  eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
357
368
  }
358
- define_method(:other_state){
359
- # If a design matrix G has columns larger than 4,
360
- # other states excluding position and time are estimated.
361
- (self.G.rows <= 4) ? [] : (self.S * self.delta_r).transpose.to_a[0][4..-1]
362
- }
363
369
  }
364
370
 
365
371
  proc{
@@ -368,7 +374,13 @@ class Receiver
368
374
  eph.svid = prn
369
375
  [prn, eph]
370
376
  }.flatten(1)]
371
- define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data|
377
+ eph_glonass_list = Hash[*(1..24).collect{|num|
378
+ eph = GPS::Ephemeris_GLONASS::new
379
+ eph.svid = num
380
+ [num, eph]
381
+ }.flatten(1)]
382
+ define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data, *options|
383
+ opt = options[0] || {}
372
384
  case sys
373
385
  when :GPS, :QZSS
374
386
  next unless eph = eph_list[prn]
@@ -398,6 +410,15 @@ class Receiver
398
410
  $stderr.puts str
399
411
  } if @debug[:SBAS_IGP]
400
412
  end
413
+ when :GLONASS
414
+ next unless eph = eph_glonass_list[prn]
415
+ leap_sec = @solver.gps_space_node.is_valid_utc ?
416
+ @solver.gps_space_node.iono_utc.delta_t_LS :
417
+ GPS::Time::guess_leap_seconds(t_meas)
418
+ next unless eph.parse(bcast_data[0..3], leap_sec)
419
+ eph.freq_ch = opt[:freq_ch] || 0
420
+ @solver.glonass_space_node.register_ephemeris(prn, eph)
421
+ eph.invalidate
401
422
  end
402
423
  }
403
424
  }.call
@@ -475,7 +496,11 @@ class Receiver
475
496
  }
476
497
  sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
477
498
  case sys
478
- when :GPS, :QZSS;
499
+ when :GPS, :SBAS, :QZSS;
500
+ when :GLONASS
501
+ svid += 0x100
502
+ meas.add(svid, :L1_FREQUENCY,
503
+ GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7))
479
504
  else; next
480
505
  end
481
506
  trk_stat = loader.call(46, 1)[0]
@@ -509,12 +534,14 @@ class Receiver
509
534
  })
510
535
  when [0x02, 0x13] # RXM-SFRBX
511
536
  sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
537
+ opt = {}
538
+ opt[:freq_ch] = packet[6 + 3] - 7 if sys == :GLONASS
512
539
  register_ephemeris(
513
540
  t_meas,
514
541
  sys, svid,
515
542
  packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
516
543
  v.pack("C*").unpack("V")[0]
517
- })
544
+ }, opt)
518
545
  end
519
546
  }
520
547
  $stderr.puts ", found packets are %s"%[ubx_kind.inspect]
@@ -524,6 +551,7 @@ class Receiver
524
551
  items = [
525
552
  @solver.gps_space_node,
526
553
  @solver.sbas_space_node,
554
+ @solver.glonass_space_node,
527
555
  ].inject(0){|res, sn|
528
556
  loaded_items = sn.send(:read, fname)
529
557
  raise "Format error! (Not RINEX) #{fname}" if loaded_items < 0
@@ -536,6 +564,7 @@ class Receiver
536
564
  after_run = b || proc{|pvt| puts pvt.to_s if pvt}
537
565
  $stderr.print "Reading RINEX observation file (%s)"%[fname]
538
566
  types = nil
567
+ glonass_freq = nil
539
568
  count = 0
540
569
  GPS::RINEX_Observation::read(fname){|item|
541
570
  $stderr.print '.' if (count += 1) % 1000 == 0
@@ -558,12 +587,32 @@ class Receiver
558
587
  }.compact]
559
588
  }.flatten(1))]
560
589
 
590
+ glonass_freq ||= proc{|spec|
591
+ # frequency channels described in observation file
592
+ next {} unless spec
593
+ Hash[*(spec.collect{|line|
594
+ line[4..-1].scan(/R(\d{2}).([\s+-]\d)./).collect{|prn, ch|
595
+ [prn.to_i, GPS::SpaceNode_GLONASS::L1_frequency(ch.to_i)]
596
+ }
597
+ }.flatten(2))]
598
+ }.call(item[:header]["GLONASS SLOT / FRQ #"])
599
+
561
600
  meas = GPS::Measurement::new
562
601
  item[:meas].each{|k, v|
563
602
  sys, prn = k
564
603
  case sys
565
604
  when 'G', ' '
605
+ when 'S'; prn += 100
566
606
  when 'J'; prn += 192
607
+ when 'R'
608
+ freq = (glonass_freq[prn] ||= proc{|sn|
609
+ # frequency channels saved with ephemeris
610
+ sn.update_all_ephemeris(t_meas)
611
+ next nil unless sn.ephemeris(prn).in_range?(t_meas)
612
+ sn.ephemeris(prn).frequency_L1
613
+ }.call(@solver.glonass_space_node))
614
+ prn += 0x100
615
+ meas.add(prn, :L1_FREQUENCY, freq) if freq
567
616
  else; next
568
617
  end
569
618
  types[sys] = (types[' '] || []) unless types[sys]
@@ -1,5 +1,5 @@
1
1
  # frozen_string_literal: true
2
2
 
3
3
  module GPS_PVT
4
- VERSION = "0.3.3"
4
+ VERSION = "0.4.0"
5
5
  end
metadata CHANGED
@@ -1,14 +1,14 @@
1
1
  --- !ruby/object:Gem::Specification
2
2
  name: gps_pvt
3
3
  version: !ruby/object:Gem::Version
4
- version: 0.3.3
4
+ version: 0.4.0
5
5
  platform: ruby
6
6
  authors:
7
7
  - fenrir(M.Naruoka)
8
8
  autorequire:
9
9
  bindir: exe
10
10
  cert_chain: []
11
- date: 2022-03-04 00:00:00.000000000 Z
11
+ date: 2022-03-17 00:00:00.000000000 Z
12
12
  dependencies:
13
13
  - !ruby/object:Gem::Dependency
14
14
  name: rake
@@ -61,7 +61,10 @@ files:
61
61
  - ext/gps_pvt/GPS/GPS_wrap.cxx
62
62
  - ext/gps_pvt/SylphideMath/SylphideMath_wrap.cxx
63
63
  - ext/gps_pvt/extconf.rb
64
+ - ext/ninja-scan-light/tool/algorithm/integral.h
64
65
  - ext/ninja-scan-light/tool/navigation/EGM.h
66
+ - ext/ninja-scan-light/tool/navigation/GLONASS.h
67
+ - ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h
65
68
  - ext/ninja-scan-light/tool/navigation/GPS.h
66
69
  - ext/ninja-scan-light/tool/navigation/GPS_Solver.h
67
70
  - ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h