gps_pvt 0.3.3 → 0.5.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 +4 -3
- data/Rakefile +2 -0
- data/exe/gps_pvt +37 -5
- 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 +1329 -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/{sig/gps_pvt.rbs → gps_pvt.rbs} +0 -0
- data/lib/gps_pvt/receiver.rb +68 -16
- data/lib/gps_pvt/util.rb +32 -0
- data/lib/gps_pvt/version.rb +1 -1
- metadata +7 -3
@@ -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
|
|
File without changes
|
data/lib/gps_pvt/receiver.rb
CHANGED
@@ -4,6 +4,7 @@ Receiver class to be an top level interface to a user
|
|
4
4
|
=end
|
5
5
|
|
6
6
|
require_relative 'GPS'
|
7
|
+
require_relative 'util'
|
7
8
|
|
8
9
|
module GPS_PVT
|
9
10
|
class Receiver
|
@@ -153,7 +154,7 @@ class Receiver
|
|
153
154
|
rel_prop
|
154
155
|
}
|
155
156
|
@debug = {}
|
156
|
-
solver_opts = [:gps_options, :sbas_options].collect{|target|
|
157
|
+
solver_opts = [:gps_options, :sbas_options, :glonass_options].collect{|target|
|
157
158
|
@solver.send(target)
|
158
159
|
}
|
159
160
|
solver_opts.each{|opt|
|
@@ -268,6 +269,11 @@ class Receiver
|
|
268
269
|
prns.each{|prn| @solver.sbas_options.send(mode, prn)}
|
269
270
|
elsif check_sys_svid.call(:QZSS, 193..202) then
|
270
271
|
[svid || (193..202).to_a].flatten.each{|prn| @solver.gps_options.send(mode, prn)}
|
272
|
+
elsif check_sys_svid.call(:GLONASS, 1..24, 0x100) then
|
273
|
+
prns = [svid || (1..24).to_a].flatten.collect{|i| (i & 0xFF) + 0x100}
|
274
|
+
labels = prns.collect{|prn| "GLONASS:#{prn & 0xFF}"}
|
275
|
+
update_output.call(:GLONASS, prns, labels)
|
276
|
+
prns.each{|prn| @solver.glonass_options.send(mode, prn & 0xFF)}
|
271
277
|
else
|
272
278
|
raise "Unknown satellite: #{spec}"
|
273
279
|
end
|
@@ -344,22 +350,23 @@ class Receiver
|
|
344
350
|
[[:@azimuth, az], [:@elevation, el]].each{|k, values|
|
345
351
|
self.instance_variable_set(k, Hash[*(sats.zip(values).flatten(1))])
|
346
352
|
}
|
353
|
+
mat_S = self.S
|
347
354
|
[:@slopeH, :@slopeV] \
|
348
|
-
.zip((self.fd ? self.slope_HV_enu.to_a.transpose : [nil, nil])) \
|
355
|
+
.zip((self.fd ? self.slope_HV_enu(mat_S).to_a.transpose : [nil, nil])) \
|
349
356
|
.each{|k, values|
|
350
357
|
self.instance_variable_set(k,
|
351
358
|
Hash[*(values ? sats.zip(values).flatten(1) : [])])
|
352
359
|
}
|
360
|
+
# If a design matrix G has columns larger than 4,
|
361
|
+
# other states excluding position and time are estimated.
|
362
|
+
@other_state = self.position_solved? \
|
363
|
+
? (mat_S * self.delta_r.partial(self.used_satellites, 1, 0, 0)).transpose.to_a[0][4..-1] \
|
364
|
+
: []
|
353
365
|
instance_variable_get(target)
|
354
366
|
}
|
355
|
-
[:azimuth, :elevation, :slopeH, :slopeV].each{|k|
|
367
|
+
[:azimuth, :elevation, :slopeH, :slopeV, :other_state].each{|k|
|
356
368
|
eval("define_method(:#{k}){@#{k} || self.post_solution(:@#{k})}")
|
357
369
|
}
|
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
370
|
}
|
364
371
|
|
365
372
|
proc{
|
@@ -368,7 +375,13 @@ class Receiver
|
|
368
375
|
eph.svid = prn
|
369
376
|
[prn, eph]
|
370
377
|
}.flatten(1)]
|
371
|
-
|
378
|
+
eph_glonass_list = Hash[*(1..24).collect{|num|
|
379
|
+
eph = GPS::Ephemeris_GLONASS::new
|
380
|
+
eph.svid = num
|
381
|
+
[num, eph]
|
382
|
+
}.flatten(1)]
|
383
|
+
define_method(:register_ephemeris){|t_meas, sys, prn, bcast_data, *options|
|
384
|
+
opt = options[0] || {}
|
372
385
|
case sys
|
373
386
|
when :GPS, :QZSS
|
374
387
|
next unless eph = eph_list[prn]
|
@@ -398,6 +411,15 @@ class Receiver
|
|
398
411
|
$stderr.puts str
|
399
412
|
} if @debug[:SBAS_IGP]
|
400
413
|
end
|
414
|
+
when :GLONASS
|
415
|
+
next unless eph = eph_glonass_list[prn]
|
416
|
+
leap_sec = @solver.gps_space_node.is_valid_utc ?
|
417
|
+
@solver.gps_space_node.iono_utc.delta_t_LS :
|
418
|
+
GPS::Time::guess_leap_seconds(t_meas)
|
419
|
+
next unless eph.parse(bcast_data[0..3], leap_sec)
|
420
|
+
eph.freq_ch = opt[:freq_ch] || 0
|
421
|
+
@solver.glonass_space_node.register_ephemeris(prn, eph)
|
422
|
+
eph.invalidate
|
401
423
|
end
|
402
424
|
}
|
403
425
|
}.call
|
@@ -475,7 +497,11 @@ class Receiver
|
|
475
497
|
}
|
476
498
|
sys, svid = gnss_serial.call(*loader.call(36, 2).reverse)
|
477
499
|
case sys
|
478
|
-
when :GPS, :QZSS;
|
500
|
+
when :GPS, :SBAS, :QZSS;
|
501
|
+
when :GLONASS
|
502
|
+
svid += 0x100
|
503
|
+
meas.add(svid, :L1_FREQUENCY,
|
504
|
+
GPS::SpaceNode_GLONASS::L1_frequency(loader.call(39, 1, "C") - 7))
|
479
505
|
else; next
|
480
506
|
end
|
481
507
|
trk_stat = loader.call(46, 1)[0]
|
@@ -509,33 +535,39 @@ class Receiver
|
|
509
535
|
})
|
510
536
|
when [0x02, 0x13] # RXM-SFRBX
|
511
537
|
sys, svid = gnss_serial.call(packet[6 + 1], packet[6])
|
538
|
+
opt = {}
|
539
|
+
opt[:freq_ch] = packet[6 + 3] - 7 if sys == :GLONASS
|
512
540
|
register_ephemeris(
|
513
541
|
t_meas,
|
514
542
|
sys, svid,
|
515
543
|
packet.slice(6 + 8, 4 * packet[6 + 4]).each_slice(4).collect{|v|
|
516
544
|
v.pack("C*").unpack("V")[0]
|
517
|
-
})
|
545
|
+
}, opt)
|
518
546
|
end
|
519
547
|
}
|
520
548
|
$stderr.puts ", found packets are %s"%[ubx_kind.inspect]
|
521
549
|
end
|
522
550
|
|
523
|
-
def parse_rinex_nav(
|
551
|
+
def parse_rinex_nav(src)
|
552
|
+
fname = Util::get_txt(src)
|
524
553
|
items = [
|
525
554
|
@solver.gps_space_node,
|
526
555
|
@solver.sbas_space_node,
|
556
|
+
@solver.glonass_space_node,
|
527
557
|
].inject(0){|res, sn|
|
528
558
|
loaded_items = sn.send(:read, fname)
|
529
|
-
raise "Format error! (Not RINEX) #{
|
559
|
+
raise "Format error! (Not RINEX) #{src}" if loaded_items < 0
|
530
560
|
res + loaded_items
|
531
561
|
}
|
532
|
-
$stderr.puts "Read RINEX NAV file (%s): %d items."%[
|
562
|
+
$stderr.puts "Read RINEX NAV file (%s): %d items."%[src, items]
|
533
563
|
end
|
534
564
|
|
535
|
-
def parse_rinex_obs(
|
565
|
+
def parse_rinex_obs(src, &b)
|
566
|
+
fname = Util::get_txt(src)
|
536
567
|
after_run = b || proc{|pvt| puts pvt.to_s if pvt}
|
537
|
-
$stderr.print "Reading RINEX observation file (%s)"%[
|
568
|
+
$stderr.print "Reading RINEX observation file (%s)"%[src]
|
538
569
|
types = nil
|
570
|
+
glonass_freq = nil
|
539
571
|
count = 0
|
540
572
|
GPS::RINEX_Observation::read(fname){|item|
|
541
573
|
$stderr.print '.' if (count += 1) % 1000 == 0
|
@@ -558,12 +590,32 @@ class Receiver
|
|
558
590
|
}.compact]
|
559
591
|
}.flatten(1))]
|
560
592
|
|
593
|
+
glonass_freq ||= proc{|spec|
|
594
|
+
# frequency channels described in observation file
|
595
|
+
next {} unless spec
|
596
|
+
Hash[*(spec.collect{|line|
|
597
|
+
line[4..-1].scan(/R(\d{2}).([\s+-]\d)./).collect{|prn, ch|
|
598
|
+
[prn.to_i, GPS::SpaceNode_GLONASS::L1_frequency(ch.to_i)]
|
599
|
+
}
|
600
|
+
}.flatten(2))]
|
601
|
+
}.call(item[:header]["GLONASS SLOT / FRQ #"])
|
602
|
+
|
561
603
|
meas = GPS::Measurement::new
|
562
604
|
item[:meas].each{|k, v|
|
563
605
|
sys, prn = k
|
564
606
|
case sys
|
565
607
|
when 'G', ' '
|
608
|
+
when 'S'; prn += 100
|
566
609
|
when 'J'; prn += 192
|
610
|
+
when 'R'
|
611
|
+
freq = (glonass_freq[prn] ||= proc{|sn|
|
612
|
+
# frequency channels saved with ephemeris
|
613
|
+
sn.update_all_ephemeris(t_meas)
|
614
|
+
next nil unless sn.ephemeris(prn).in_range?(t_meas)
|
615
|
+
sn.ephemeris(prn).frequency_L1
|
616
|
+
}.call(@solver.glonass_space_node))
|
617
|
+
prn += 0x100
|
618
|
+
meas.add(prn, :L1_FREQUENCY, freq) if freq
|
567
619
|
else; next
|
568
620
|
end
|
569
621
|
types[sys] = (types[' '] || []) unless types[sys]
|
data/lib/gps_pvt/util.rb
ADDED
@@ -0,0 +1,32 @@
|
|
1
|
+
require 'open-uri'
|
2
|
+
require 'tempfile'
|
3
|
+
require 'uri'
|
4
|
+
require 'zlib'
|
5
|
+
|
6
|
+
module GPS_PVT
|
7
|
+
module Util
|
8
|
+
class << self
|
9
|
+
def inflate(src)
|
10
|
+
Zlib::GzipReader.send(*(src.kind_of?(IO) ? [:new, src] : [:open, src]))
|
11
|
+
end
|
12
|
+
def get_txt(fname_or_uri)
|
13
|
+
is_uri = fname_or_uri.kind_of?(URI)
|
14
|
+
((is_uri && (RUBY_VERSION >= "2.5.0")) ? URI : Kernel) \
|
15
|
+
.send(:open, fname_or_uri){|src|
|
16
|
+
is_gz = (src.content_type =~ /gzip/) if is_uri
|
17
|
+
is_gz ||= (fname_or_uri.to_s =~ /\.gz$/)
|
18
|
+
is_file = src.kind_of?(File) || src.kind_of?(Tempfile)
|
19
|
+
|
20
|
+
return src.path if ((!is_gz) and is_file)
|
21
|
+
|
22
|
+
Tempfile::open(File::basename($0, '.*')){|dst|
|
23
|
+
dst.binmode
|
24
|
+
dst.write((is_gz ? inflate(is_file ? src.path : src) : src).read)
|
25
|
+
dst.rewind
|
26
|
+
dst.path
|
27
|
+
}
|
28
|
+
}
|
29
|
+
end
|
30
|
+
end
|
31
|
+
end
|
32
|
+
end
|
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.5.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-
|
11
|
+
date: 2022-04-11 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
|
@@ -90,11 +93,12 @@ files:
|
|
90
93
|
- ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb
|
91
94
|
- ext/ninja-scan-light/tool/swig/spec/SylphideMath_spec.rb
|
92
95
|
- gps_pvt.gemspec
|
96
|
+
- gps_pvt.rbs
|
93
97
|
- lib/gps_pvt.rb
|
94
98
|
- lib/gps_pvt/receiver.rb
|
95
99
|
- lib/gps_pvt/ubx.rb
|
100
|
+
- lib/gps_pvt/util.rb
|
96
101
|
- lib/gps_pvt/version.rb
|
97
|
-
- sig/gps_pvt.rbs
|
98
102
|
homepage: https://github.com/fenrir-naru/gps_pvt
|
99
103
|
licenses: []
|
100
104
|
metadata:
|