gps_pvt 0.3.3 → 0.4.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.
- checksums.yaml +4 -4
- data/README.md +2 -2
- data/Rakefile +2 -0
- data/exe/gps_pvt +2 -2
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +5595 -131
- data/ext/ninja-scan-light/tool/algorithm/integral.h +91 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +1270 -0
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +306 -0
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_Base.h +7 -0
- data/ext/ninja-scan-light/tool/navigation/RINEX.h +389 -4
- data/ext/ninja-scan-light/tool/swig/GPS.i +217 -6
- data/lib/gps_pvt/receiver.rb +60 -11
- data/lib/gps_pvt/version.rb +1 -1
- metadata +5 -2
@@ -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
|
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
|
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
|
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
|
|
data/lib/gps_pvt/receiver.rb
CHANGED
@@ -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
|
-
|
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]
|
data/lib/gps_pvt/version.rb
CHANGED
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.
|
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-
|
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
|