gps_pvt 0.7.2 → 0.8.1
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/README.md +1 -1
- data/Rakefile +1 -0
- data/exe/gps_pvt +3 -18
- data/exe/to_ubx +103 -8
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +1136 -65
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +137 -9
- data/ext/ninja-scan-light/tool/navigation/GPS.h +301 -50
- data/ext/ninja-scan-light/tool/navigation/QZSS.h +48 -15
- data/ext/ninja-scan-light/tool/navigation/SBAS.h +106 -4
- data/ext/ninja-scan-light/tool/param/bit_array.h +53 -16
- data/ext/ninja-scan-light/tool/swig/GPS.i +228 -24
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +61 -0
- data/ext/ninja-scan-light/tool/util/bit_counter.h +84 -0
- data/gps_pvt.gemspec +1 -0
- data/lib/gps_pvt/receiver/extension.rb +51 -0
- data/lib/gps_pvt/receiver.rb +79 -56
- data/lib/gps_pvt/util.rb +33 -1
- data/lib/gps_pvt/version.rb +1 -1
- metadata +18 -2
@@ -46,6 +46,7 @@
|
|
46
46
|
|
47
47
|
#include "GPS.h"
|
48
48
|
#include "algorithm/integral.h"
|
49
|
+
#include "util/bit_counter.h"
|
49
50
|
|
50
51
|
template <class FloatT = double>
|
51
52
|
class GLONASS_SpaceNode {
|
@@ -89,17 +90,79 @@ static u ## bits ## _t name(const InputT *buf){ \
|
|
89
90
|
return \
|
90
91
|
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
91
92
|
buf, offset_bits, length); \
|
93
|
+
} \
|
94
|
+
static void name ## _set(InputT *dest, const u ## bits ## _t &src){ \
|
95
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
96
|
+
dest, src, offset_bits, length, EffectiveBits, PaddingBits_MSB); \
|
92
97
|
}
|
93
98
|
#define convert_s(bits, offset_bits, length, name) \
|
94
99
|
static s ## bits ## _t name(const InputT *buf){ \
|
95
100
|
u ## bits ## _t temp( \
|
96
101
|
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
97
102
|
buf, offset_bits, length)); \
|
98
|
-
|
103
|
+
/* MSB is sign bit (Not equal to two's complement?) */ \
|
104
|
+
static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \
|
99
105
|
return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \
|
106
|
+
} \
|
107
|
+
static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
108
|
+
static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \
|
109
|
+
u ## bits ## _t src2((src < 0) ? ((-src) | mask) : src); \
|
110
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
111
|
+
dest, src2, offset_bits, length, EffectiveBits, PaddingBits_MSB); \
|
100
112
|
}
|
113
|
+
convert_u( 8, 0, 1, idle);
|
114
|
+
static void idle_set(InputT *dest){idle_set(dest, 0);}
|
101
115
|
convert_u( 8, 1, 4, m);
|
102
116
|
convert_u( 8, 77, 8, KX);
|
117
|
+
static void KX_set(InputT *dest){
|
118
|
+
#if 0
|
119
|
+
// mask generation with Ruby
|
120
|
+
mask_bits = 32
|
121
|
+
mask_str = "0x%0#{mask_bits / 4}X"
|
122
|
+
[
|
123
|
+
[9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43,
|
124
|
+
45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84],
|
125
|
+
[9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44,
|
126
|
+
45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84],
|
127
|
+
[10..12, 16..19, 23..26, 31..34, 38..41, 46..49, 54..57, 62..65, 69..72, 77..80, 85],
|
128
|
+
[13..19, 27..34, 42..49, 58..65, 73..80],
|
129
|
+
[20..34, 50..65, 81..85],
|
130
|
+
[35..65],
|
131
|
+
[66..85],
|
132
|
+
].collect{|pat|
|
133
|
+
pat.collect{|idx| idx.to_a rescue idx}.flatten \
|
134
|
+
.collect{|i| 85 - i}.sort.chunk{|i| i / mask_bits} \
|
135
|
+
.collect{|quot, i_s|
|
136
|
+
mask = eval("0b#{i_s.inject("0" * mask_bits){|res, i| res[i % mask_bits] = '1'; res}}")
|
137
|
+
#mask = [mask_be].pack("N").unpack("L")[0] # endianess correction
|
138
|
+
[quot, mask] # endianess correction
|
139
|
+
}.inject([mask_str % [0]] * (77.0 / mask_bits).ceil){|res, (i, mask)|
|
140
|
+
res[i] = (mask_str % [mask]); res
|
141
|
+
}
|
142
|
+
}.transpose
|
143
|
+
#endif
|
144
|
+
u8_t hamming(0);
|
145
|
+
static const struct {
|
146
|
+
uint_t idx, len;
|
147
|
+
u32_t mask[8];
|
148
|
+
} prop[] = {
|
149
|
+
{ 0, 32, {0x55555AAAu, 0x66666CCCu, 0x87878F0Fu, 0x07F80FF0u, 0xF8000FFFu, 0x00000FFFu, 0xFFFFF000u, ~0u}},
|
150
|
+
{32, 32, {0xAAAAB555u, 0xCCCCD999u, 0x0F0F1E1Eu, 0x0FF01FE0u, 0xF0001FFFu, 0xFFFFE000u, 0, ~0u}},
|
151
|
+
{64, 13, {0x6AD80000u >> 19, 0xB3680000u >> 19, 0x3C700000u >> 19, 0x3F800000u >> 19, 0xC0000000u >> 19, 0, 0, ~0u}},
|
152
|
+
};
|
153
|
+
for(std::size_t j(0); j < sizeof(prop) / sizeof(prop[0]); ++j){
|
154
|
+
u8_t check_bit(1);
|
155
|
+
u32_t v(DataParser::template bits2num<u32_t, EffectiveBits, PaddingBits_MSB>(dest, prop[j].idx, prop[j].len));
|
156
|
+
for(int i(0); i < 8; ++i, check_bit <<= 1){
|
157
|
+
if(BitCounter<u32_t>::count(v & prop[j].mask[i]) % 2 == 0){continue;}
|
158
|
+
hamming ^= check_bit;
|
159
|
+
}
|
160
|
+
}
|
161
|
+
if(BitCounter<u8_t>::count(hamming & 0x7F) % 2 == 1){
|
162
|
+
hamming ^= 0x80;
|
163
|
+
}
|
164
|
+
KX_set(dest, hamming);
|
165
|
+
}
|
103
166
|
|
104
167
|
struct String1 {
|
105
168
|
convert_u( 8, 7, 2, P1);
|
@@ -135,7 +198,7 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
135
198
|
convert_u( 8, 70, 5, n);
|
136
199
|
convert_u( 8, 75, 2, M);
|
137
200
|
};
|
138
|
-
struct
|
201
|
+
struct String5_Almanac {
|
139
202
|
convert_u(16, 5, 11, NA);
|
140
203
|
convert_s(32, 16, 32, tau_c);
|
141
204
|
convert_u( 8, 49, 5, N_4);
|
@@ -143,7 +206,7 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
143
206
|
convert_u( 8, 76, 1, l_n);
|
144
207
|
};
|
145
208
|
|
146
|
-
struct
|
209
|
+
struct String6_Almanac { // String 6; same as 8, 10, 12, 14
|
147
210
|
convert_u( 8, 5, 1, C_n);
|
148
211
|
convert_u( 8, 6, 2, M_n);
|
149
212
|
convert_u( 8, 8, 5, nA);
|
@@ -152,7 +215,7 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
152
215
|
convert_s(32, 44, 18, delta_iA_n);
|
153
216
|
convert_u(16, 62, 15, epsilonA_n);
|
154
217
|
};
|
155
|
-
struct
|
218
|
+
struct String7_Almanac { // String 7; same as 9, 11, 13, 15
|
156
219
|
convert_s(16, 5, 16, omegaA_n);
|
157
220
|
convert_u(32, 21, 21, tA_lambda_n);
|
158
221
|
convert_s(32, 42, 22, delta_TA_n);
|
@@ -257,7 +320,7 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
257
320
|
bool l_n;
|
258
321
|
#define fetch_item(name) name = BroadcastedMessage< \
|
259
322
|
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
|
260
|
-
::
|
323
|
+
:: String5_Almanac :: name (src)
|
261
324
|
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
262
325
|
void update_string5(const InputT *src){
|
263
326
|
fetch_item(tau_c);
|
@@ -267,6 +330,22 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
267
330
|
fetch_item(l_n);
|
268
331
|
}
|
269
332
|
#undef fetch_item
|
333
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
334
|
+
void dump(BufferT *dst){
|
335
|
+
typedef BroadcastedMessage<
|
336
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
337
|
+
deparse_t;
|
338
|
+
#define dump_item(name) deparse_t::String5_Almanac:: name ## _set(dst, name)
|
339
|
+
dump_item(tau_c);
|
340
|
+
dump_item(tau_GPS);
|
341
|
+
dump_item(N_4);
|
342
|
+
dump_item(NA);
|
343
|
+
dump_item(l_n);
|
344
|
+
#undef dump_item
|
345
|
+
deparse_t::m_set(dst, 5);
|
346
|
+
deparse_t::idle_set(dst);
|
347
|
+
deparse_t::KX_set(dst);
|
348
|
+
}
|
270
349
|
|
271
350
|
enum {
|
272
351
|
SF_tau_c, SF_tau_GPS,
|
@@ -307,7 +386,7 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
307
386
|
|
308
387
|
raw_t &operator=(const TimeProperties &t){
|
309
388
|
#define CONVERT(TARGET) \
|
310
|
-
{TARGET = (s32_t)(
|
389
|
+
{TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);}
|
311
390
|
CONVERT(tau_c);
|
312
391
|
CONVERT(tau_GPS);
|
313
392
|
std::div_t divmod(std::div(t.date.year - 1996, 4));
|
@@ -364,7 +443,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
364
443
|
uint_t E_n; // [days]
|
365
444
|
uint_t P1; // [s] time interval of two adjacent t_b; 0 - 0, 1 - 30, 2 - 45, 3 - 60 mins
|
366
445
|
bool P2;
|
367
|
-
//bool P3; // flag for
|
446
|
+
//bool P3; // flag for almanac; 1 - five, 0 - four
|
368
447
|
bool P4; // flag for ephemeris; 1 - uploaded by the control segment
|
369
448
|
bool l_n; // health flag; 0 - healthy, 1 - malfunction
|
370
449
|
|
@@ -798,6 +877,56 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
798
877
|
fetch_item(4, M);
|
799
878
|
}
|
800
879
|
#undef fetch_item
|
880
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
881
|
+
void dump(BufferT *dst, const unsigned int &str){
|
882
|
+
typedef BroadcastedMessage<
|
883
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
884
|
+
deparse_t;
|
885
|
+
#define dump_item(num, name) deparse_t::String ## num :: name ## _set(dst, name)
|
886
|
+
switch(str){
|
887
|
+
case 1:
|
888
|
+
dump_item(1, P1);
|
889
|
+
dump_item(1, t_k);
|
890
|
+
dump_item(1, xn_dot);
|
891
|
+
dump_item(1, xn_ddot);
|
892
|
+
dump_item(1, xn);
|
893
|
+
break;
|
894
|
+
case 2: {
|
895
|
+
dump_item(2, B_n);
|
896
|
+
dump_item(2, P2);
|
897
|
+
dump_item(2, t_b);
|
898
|
+
dump_item(2, yn_dot);
|
899
|
+
dump_item(2, yn_ddot);
|
900
|
+
dump_item(2, yn);
|
901
|
+
break;
|
902
|
+
}
|
903
|
+
case 3:
|
904
|
+
dump_item(3, P3);
|
905
|
+
dump_item(3, gamma_n);
|
906
|
+
dump_item(3, p);
|
907
|
+
dump_item(3, l_n);
|
908
|
+
dump_item(3, zn_dot);
|
909
|
+
dump_item(3, zn_ddot);
|
910
|
+
dump_item(3, zn);
|
911
|
+
break;
|
912
|
+
case 4:
|
913
|
+
dump_item(4, tau_n);
|
914
|
+
dump_item(4, delta_tau_n);
|
915
|
+
dump_item(4, E_n);
|
916
|
+
dump_item(4, P4);
|
917
|
+
dump_item(4, F_T);
|
918
|
+
dump_item(4, N_T);
|
919
|
+
dump_item(4, n);
|
920
|
+
dump_item(4, M);
|
921
|
+
break;
|
922
|
+
default:
|
923
|
+
return;
|
924
|
+
}
|
925
|
+
#undef dump_item
|
926
|
+
deparse_t::m_set(dst, str);
|
927
|
+
deparse_t::idle_set(dst);
|
928
|
+
deparse_t::KX_set(dst);
|
929
|
+
}
|
801
930
|
|
802
931
|
enum {
|
803
932
|
SF_xn, SF_yn = SF_xn, SF_zn = SF_xn,
|
@@ -859,9 +988,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
859
988
|
return res;
|
860
989
|
}
|
861
990
|
raw_t &operator=(const Ephemeris &eph){
|
862
|
-
// TODO: m?
|
863
991
|
#define CONVERT(TARGET) \
|
864
|
-
{TARGET = (s32_t)(
|
992
|
+
{TARGET = (s32_t)std::floor(eph.TARGET / sf[SF_ ## TARGET] + 0.5);}
|
865
993
|
svid = eph.svid;
|
866
994
|
{ // t_k
|
867
995
|
std::div_t minutes(div(eph.t_k, 60));
|