gps_pvt 0.3.3 → 0.5.0
Sign up to get free protection for your applications and to get access to all the features.
- 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:
|