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
@@ -201,6 +201,11 @@ template <class InputT> \
|
|
201
201
|
static u ## bits ## _t name(const InputT *buf){ \
|
202
202
|
return parser_t::template bits2num<u ## bits ## _t, InputT>( \
|
203
203
|
buf, offset_bits, length); \
|
204
|
+
} \
|
205
|
+
template <class BufferT> \
|
206
|
+
static void name ## _set(BufferT *dest, const u ## bits ## _t &src){ \
|
207
|
+
parser_t::template num2bits<u ## bits ## _t, BufferT>( \
|
208
|
+
dest, src, offset_bits, length); \
|
204
209
|
}
|
205
210
|
#define convert_s(bits, offset_bits, length, name) \
|
206
211
|
template <class InputT> \
|
@@ -208,12 +213,22 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
208
213
|
return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t, InputT>( \
|
209
214
|
buf, offset_bits) \
|
210
215
|
>> (bits - length); \
|
216
|
+
} \
|
217
|
+
template <class BufferT> \
|
218
|
+
static void name ## _set(BufferT *dest, const s ## bits ## _t &src){ \
|
219
|
+
parser_t::template num2bits<u ## bits ## _t, BufferT>( \
|
220
|
+
dest, *(u ## bits ## _t *)(&src), offset_bits, length); \
|
211
221
|
}
|
212
222
|
#define convert_u_ch(bits, offset_bits, length, bits_per_ch, name) \
|
213
223
|
template <class InputT> \
|
214
224
|
static u ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
|
215
225
|
return parser_t::template bits2num<u ## bits ## _t>( \
|
216
226
|
buf, offset_bits + (bits_per_ch * ch), length); \
|
227
|
+
} \
|
228
|
+
template <class BufferT> \
|
229
|
+
static void name ## _set(BufferT *dest, const uint_t &ch, const u ## bits ## _t &src){ \
|
230
|
+
parser_t::template num2bits<u ## bits ## _t, BufferT>( \
|
231
|
+
dest, src, offset_bits + (bits_per_ch * ch), length); \
|
217
232
|
}
|
218
233
|
#define convert_s_ch(bits, offset_bits, length, bits_per_ch, name) \
|
219
234
|
template <class InputT> \
|
@@ -221,6 +236,11 @@ static s ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
|
|
221
236
|
return (s ## bits ## _t)parser_t::template bits2num<u ## bits ## _t>( \
|
222
237
|
buf, offset_bits + (bits_per_ch * ch)) \
|
223
238
|
>> (bits - length); \
|
239
|
+
} \
|
240
|
+
template <class BufferT> \
|
241
|
+
static void name ## _set(BufferT *dest, const uint_t &ch, const s ## bits ## _t &src){ \
|
242
|
+
parser_t::template num2bits<u ## bits ## _t, BufferT>( \
|
243
|
+
dest, *(u ## bits ## _t *)(&src), offset_bits + (bits_per_ch * ch), length); \
|
224
244
|
}
|
225
245
|
|
226
246
|
/**
|
@@ -254,8 +274,55 @@ static s ## bits ## _t name(const InputT *buf, const uint_t &ch){ \
|
|
254
274
|
}
|
255
275
|
|
256
276
|
convert_u(8, 0, 8, preamble);
|
277
|
+
template <class BufferT>
|
278
|
+
static void preamble_set2(BufferT *dest, const unsigned int &index = 0){
|
279
|
+
static const u8_t preamble_list[] = {0x53, 0x9A, 0xC6};
|
280
|
+
preamble_set(dest, preamble_list[index % 3]);
|
281
|
+
}
|
257
282
|
convert_u(8, 8, 6, message_type);
|
258
283
|
|
284
|
+
convert_u(32, 226, 24, parity);
|
285
|
+
/**
|
286
|
+
* update parity bits based on current buffer
|
287
|
+
* @param buf buffer
|
288
|
+
* @see SBAS MOPS (DO-229) A.4.3.3
|
289
|
+
*/
|
290
|
+
template <class BufferT>
|
291
|
+
static void parity_set(BufferT *dst){
|
292
|
+
// CRC-24Q
|
293
|
+
static const u32_t poly(0x1864CFBu);
|
294
|
+
static const struct tbl_t {
|
295
|
+
u32_t fw[0x100];
|
296
|
+
tbl_t() {
|
297
|
+
for(int i(0); i < 0x100; ++i){
|
298
|
+
fw[i] = i << 16;
|
299
|
+
for(int j(0); j < 8; ++j){
|
300
|
+
fw[i] <<= 1;
|
301
|
+
if(fw[i] & 0x1000000u){fw[i] ^= poly;}
|
302
|
+
}
|
303
|
+
}
|
304
|
+
}
|
305
|
+
} tbl;
|
306
|
+
u32_t crc(0);
|
307
|
+
u8_t buf;
|
308
|
+
int bit_idx(0);
|
309
|
+
for(int bytes(226 / 8); bytes > 0; bytes--, bit_idx += 8){
|
310
|
+
buf = parser_t::template bits2num<u8_t, BufferT>(dst, bit_idx, 8);
|
311
|
+
crc = (((crc << 8) & 0xFFFF00) | buf) ^ tbl.fw[(crc >> 16) & 0xFF];
|
312
|
+
}
|
313
|
+
buf = parser_t::template bits2num<u8_t, BufferT>(dst, bit_idx, 8);
|
314
|
+
for(unsigned char mask(0x80); bit_idx < 226; bit_idx++, mask >>= 1){
|
315
|
+
crc <<= 1;
|
316
|
+
if(buf & mask){crc |= 1;}
|
317
|
+
if(crc & 0x1000000u){crc ^= poly;}
|
318
|
+
}
|
319
|
+
for(int i(0); i < 3; i++){ // forward operation for padding
|
320
|
+
unsigned int cache(tbl.fw[(crc >> 16) & 0xFF]);
|
321
|
+
crc = (((crc << 8) ^ cache) & 0xFFFF00) | (cache & 0xFF);
|
322
|
+
}
|
323
|
+
parity_set(dst, crc);
|
324
|
+
}
|
325
|
+
|
259
326
|
struct Type1 { ///< @see Fig. A-6 PRN_MASK
|
260
327
|
struct mask_t {
|
261
328
|
u8_t valid;
|
@@ -1838,6 +1905,18 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1838
1905
|
};
|
1839
1906
|
return res;
|
1840
1907
|
}
|
1908
|
+
template <class BufferT>
|
1909
|
+
void dump(BufferT *dst){
|
1910
|
+
typedef typename DataBlock::Type9 msg_t;
|
1911
|
+
#define dump_item(name) msg_t:: name ## _set(dst, name)
|
1912
|
+
DataBlock::message_type_set(dst, 9);
|
1913
|
+
msg_t::t0_set(dst, t_0); msg_t::ura_set(dst, URA);
|
1914
|
+
dump_item(x); dump_item(y); dump_item(z);
|
1915
|
+
dump_item(dx); dump_item(dy); dump_item(dz);
|
1916
|
+
dump_item(ddx); dump_item(ddy); dump_item(ddz);
|
1917
|
+
dump_item(a_Gf0); dump_item(a_Gf1);
|
1918
|
+
#undef dump_item
|
1919
|
+
}
|
1841
1920
|
|
1842
1921
|
enum {
|
1843
1922
|
SF_t_0,
|
@@ -1873,13 +1952,13 @@ sf[SF_ ## TARGET] * msg_t::TARGET(buf)
|
|
1873
1952
|
|
1874
1953
|
raw_t &operator=(const Ephemeris &eph){
|
1875
1954
|
#define CONVERT3(TARGET_DST, TARGET_SRC, TARGET_SF) \
|
1876
|
-
{TARGET_DST = (
|
1877
|
-
#define CONVERT2(TARGET, TARGET_SF) CONVERT3(
|
1955
|
+
{TARGET_DST = std::floor((TARGET_SRC / sf[SF_ ## TARGET_SF]) + 0.5);}
|
1956
|
+
#define CONVERT2(TARGET, TARGET_SF) CONVERT3(TARGET, eph.TARGET, TARGET_SF)
|
1878
1957
|
#define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
|
1879
1958
|
svid = eph.svid;
|
1880
1959
|
|
1881
1960
|
URA = URA_index(eph.URA);
|
1882
|
-
CONVERT3(t_0, std::fmod(t_0, gps_time_t::seconds_day), t_0);
|
1961
|
+
CONVERT3(t_0, std::fmod(eph.t_0, gps_time_t::seconds_day), t_0);
|
1883
1962
|
CONVERT2(x, xy); CONVERT2(y, xy); CONVERT(z);
|
1884
1963
|
CONVERT2(dx, dxy); CONVERT2(dy, dxy); CONVERT(dz);
|
1885
1964
|
CONVERT2(ddx, ddxy); CONVERT2(ddy, ddxy); CONVERT(ddz);
|
@@ -1992,7 +2071,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
|
|
1992
2071
|
float_t dx, dy, dz; ///< ECEF velocity (m/s)
|
1993
2072
|
float_t t_0; ///< Time of applicability (s)
|
1994
2073
|
|
1995
|
-
///<
|
2074
|
+
///< Upgrade to ephemeris
|
1996
2075
|
operator Ephemeris() const {
|
1997
2076
|
Ephemeris converted = {
|
1998
2077
|
prn, // Satellite number
|
@@ -2006,6 +2085,14 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
|
|
2006
2085
|
};
|
2007
2086
|
return converted;
|
2008
2087
|
}
|
2088
|
+
///< Downgrade from ephemeris
|
2089
|
+
Almanac &operator=(const Ephemeris &eph) {
|
2090
|
+
prn = eph.svid;
|
2091
|
+
t_0 = eph.t_0;
|
2092
|
+
x = eph.x; y = eph.y; z = eph.z;
|
2093
|
+
dx = eph.dx; dy = eph.dy; dz = eph.dz;
|
2094
|
+
return *this;
|
2095
|
+
}
|
2009
2096
|
|
2010
2097
|
struct raw_t {
|
2011
2098
|
u8_t data_id;
|
@@ -2028,6 +2115,21 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET_SF]){break;}
|
|
2028
2115
|
};
|
2029
2116
|
return res;
|
2030
2117
|
}
|
2118
|
+
template <class BufferT>
|
2119
|
+
void dump(BufferT *dst, const uint_t &ch = 0){
|
2120
|
+
typedef typename DataBlock::Type17 msg_t;
|
2121
|
+
#define dump_item(name) msg_t:: name ## _set(dst, ch, name)
|
2122
|
+
if(ch == 0){
|
2123
|
+
DataBlock::message_type_set(dst, 17);
|
2124
|
+
msg_t::t0_set(dst, t_0);
|
2125
|
+
}
|
2126
|
+
msg_t::id_set(dst, ch, data_id);
|
2127
|
+
dump_item(prn);
|
2128
|
+
msg_t::health_status_set(dst, ch, SV_health);
|
2129
|
+
dump_item(x); dump_item(y); dump_item(z);
|
2130
|
+
dump_item(dx); dump_item(dy); dump_item(dz);
|
2131
|
+
#undef dump_item
|
2132
|
+
}
|
2031
2133
|
|
2032
2134
|
enum {
|
2033
2135
|
SF_xy, SF_z,
|
@@ -43,25 +43,31 @@
|
|
43
43
|
|
44
44
|
#include <vector>
|
45
45
|
|
46
|
-
|
47
|
-
struct const_div_t : public std::div_t {
|
48
|
-
const_div_t(const int &num) : std::div_t(std::div(num, denom)) {}
|
49
|
-
};
|
50
|
-
template <int denom, int pow2, int log2>
|
51
|
-
struct const_div_t<denom, pow2, log2, 0> : public const_div_t<denom, (pow2 << 1), log2 + 1> {
|
52
|
-
const_div_t(const int &num) : const_div_t<denom, (pow2 << 1), log2 + 1>(num) {}
|
53
|
-
};
|
54
|
-
template <int denom, int log2>
|
55
|
-
struct const_div_t<denom, denom, log2, 0> {
|
56
|
-
int quot, rem;
|
57
|
-
const_div_t(const int &num) : quot(num >> log2), rem(num & (denom - 1)) {}
|
58
|
-
};
|
46
|
+
#include "util/bit_counter.h"
|
59
47
|
|
60
48
|
template <int MAX_SIZE, class ContainerT = unsigned char>
|
61
49
|
struct BitArray {
|
50
|
+
|
51
|
+
template <int denom, int pow2 = 1, int log2 = 0, int rem2 = denom % pow2>
|
52
|
+
struct const_div_t : public std::div_t {
|
53
|
+
const_div_t(const int &num) : std::div_t(std::div(num, denom)) {}
|
54
|
+
};
|
55
|
+
template <int denom, int pow2, int log2>
|
56
|
+
struct const_div_t<denom, pow2, log2, 0> : public const_div_t<denom, (pow2 << 1), log2 + 1> {
|
57
|
+
const_div_t(const int &num) : const_div_t<denom, (pow2 << 1), log2 + 1>(num) {}
|
58
|
+
};
|
59
|
+
template <int denom, int log2>
|
60
|
+
struct const_div_t<denom, denom, log2, 0> {
|
61
|
+
int quot, rem;
|
62
|
+
const_div_t(const int &num) : quot(num >> log2), rem(num & (denom - 1)) {}
|
63
|
+
};
|
64
|
+
|
62
65
|
static const int bits_per_addr = (int)sizeof(ContainerT) * CHAR_BIT;
|
63
66
|
static const int max_size = MAX_SIZE;
|
67
|
+
static const const_div_t<bits_per_addr> prop;
|
64
68
|
ContainerT buf[(MAX_SIZE + bits_per_addr - 1) / bits_per_addr];
|
69
|
+
static const ContainerT mask_rem;
|
70
|
+
|
65
71
|
void set(const bool &new_bit = false) {
|
66
72
|
std::memset(buf, (new_bit ? (~0) : 0), sizeof(buf));
|
67
73
|
}
|
@@ -125,12 +131,35 @@ struct BitArray {
|
|
125
131
|
return (unsigned int)res;
|
126
132
|
}
|
127
133
|
}
|
134
|
+
int ones() const {
|
135
|
+
int res(0);
|
136
|
+
for(int i(0); i < prop.quot; ++i){
|
137
|
+
res += BitCounter<ContainerT>::count(buf[i]);
|
138
|
+
}
|
139
|
+
if(prop.rem){
|
140
|
+
res += BitCounter<ContainerT>::count(
|
141
|
+
buf[(sizeof(buf) / sizeof(buf[0])) - 1] & mask_rem);
|
142
|
+
}
|
143
|
+
return res;
|
144
|
+
}
|
145
|
+
int least_index_one() const {
|
146
|
+
int res(0);
|
147
|
+
for(int i(0); prop.quot; ++i){
|
148
|
+
int ntz(BitCounter<ContainerT>::ntz(buf[i]));
|
149
|
+
res += ntz;
|
150
|
+
if(ntz < bits_per_addr){return res;}
|
151
|
+
}
|
152
|
+
if(prop.rem){
|
153
|
+
res += BitCounter<ContainerT>::ntz(
|
154
|
+
buf[(sizeof(buf) / sizeof(buf[0])) - 1] & mask_rem);
|
155
|
+
}
|
156
|
+
return res;
|
157
|
+
}
|
128
158
|
std::vector<int> indices_one(const int &offset = 0) const {
|
129
159
|
std::vector<int> res;
|
130
160
|
int idx(offset);
|
131
|
-
|
132
|
-
|
133
|
-
for(; i < qr.quot; ++i, idx += bits_per_addr){
|
161
|
+
int rem(prop.rem), i(0);
|
162
|
+
for(; i < prop.quot; ++i, idx += bits_per_addr){
|
134
163
|
int idx2(idx);
|
135
164
|
for(ContainerT temp(buf[i]); temp > 0; temp >>= 1, ++idx2){
|
136
165
|
if(temp & 0x1){res.push_back(idx2);}
|
@@ -143,4 +172,12 @@ struct BitArray {
|
|
143
172
|
}
|
144
173
|
};
|
145
174
|
|
175
|
+
template <int MAX_SIZE, class ContainerT>
|
176
|
+
const typename BitArray<MAX_SIZE, ContainerT>::template const_div_t<BitArray<MAX_SIZE, ContainerT>::bits_per_addr>
|
177
|
+
BitArray<MAX_SIZE, ContainerT>::prop
|
178
|
+
= typename BitArray<MAX_SIZE, ContainerT>::template const_div_t<BitArray<MAX_SIZE, ContainerT>::bits_per_addr>(MAX_SIZE);
|
179
|
+
|
180
|
+
template <int MAX_SIZE, class ContainerT>
|
181
|
+
const ContainerT BitArray<MAX_SIZE, ContainerT>::mask_rem = ((ContainerT)0x1 << prop.rem) - 1;
|
182
|
+
|
146
183
|
#endif /* __BIT_ARRAY_H__ */
|
@@ -273,6 +273,25 @@ struct GPS_Ionospheric_UTC_Parameters : public GPS_SpaceNode<FloatT>::Ionospheri
|
|
273
273
|
(typename GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters &)res = raw;
|
274
274
|
return res;
|
275
275
|
}
|
276
|
+
%typemap(in,numinputs=0) unsigned int buf_brdc[ANY] (unsigned int temp[$1_dim0] = {0}) "$1 = temp;"
|
277
|
+
%typemap(argout) unsigned int buf_brdc[ANY] {
|
278
|
+
for(int i(0); i < $1_dim0; ++i){
|
279
|
+
%append_output(SWIG_From(unsigned int)(($1)[i]));
|
280
|
+
}
|
281
|
+
}
|
282
|
+
/**
|
283
|
+
* Return broadcasted raw data related to ionospheric and UTC parameters.
|
284
|
+
* @param buf_brdc pointer to store raw data of subframe 4 page 18.
|
285
|
+
* Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
|
286
|
+
* @param t GPS time at broadcasting
|
287
|
+
*/
|
288
|
+
void dump(unsigned int buf_brdc[10], const GPS_Time<FloatT> &t){
|
289
|
+
typedef typename GPS_SpaceNode<FloatT>
|
290
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
291
|
+
dump_t::how_set(buf_brdc, t);
|
292
|
+
GPS_SpaceNode<FloatT>::Ionospheric_UTC_Parameters::raw_t raw;
|
293
|
+
(raw = *self).dump<2, 0>(buf_brdc);
|
294
|
+
}
|
276
295
|
}
|
277
296
|
|
278
297
|
%inline %{
|
@@ -287,6 +306,11 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
287
306
|
|| (this->iode != this->iode_subframe3)
|
288
307
|
|| ((this->iodc & 0xFF) != this->iode));
|
289
308
|
}
|
309
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
310
|
+
return is_consistent() && GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
|
311
|
+
}
|
312
|
+
GPS_Time<FloatT> t_clock() const {return GPS_Time<FloatT>(this->WN, this->t_oc);}
|
313
|
+
GPS_Time<FloatT> t_ephemeris() const {return GPS_Time<FloatT>(this->WN, this->t_oe);}
|
290
314
|
GPS_Ephemeris() : GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {
|
291
315
|
invalidate();
|
292
316
|
}
|
@@ -303,7 +327,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
303
327
|
MAKE_ACCESSOR(svid, unsigned int);
|
304
328
|
|
305
329
|
MAKE_ACCESSOR(WN, unsigned int);
|
306
|
-
MAKE_ACCESSOR(URA,
|
330
|
+
MAKE_ACCESSOR(URA, FloatT);
|
307
331
|
MAKE_ACCESSOR(SV_health, unsigned int);
|
308
332
|
MAKE_ACCESSOR(iodc, int);
|
309
333
|
MAKE_ACCESSOR(t_GD, FloatT);
|
@@ -384,6 +408,83 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
384
408
|
break;
|
385
409
|
}
|
386
410
|
}
|
411
|
+
%apply unsigned int buf_brdc[ANY] {
|
412
|
+
unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10]};
|
413
|
+
/**
|
414
|
+
* Return broadcasted raw data of ephemeris data.
|
415
|
+
* @param buf_sf1 pointer to store raw data of subframe 1.
|
416
|
+
* Each 30bit length word (MSB 2 bits are padding) is stored in each successive address of the pointer.
|
417
|
+
* @param buf_sf2 pointer to store raw data of subframe 2. Its structue is same as sf1.
|
418
|
+
* @param buf_sf3 pointer to store raw data of subframe 3. Its structue is same as sf1.
|
419
|
+
* @param t GPS time at broadcasting
|
420
|
+
*/
|
421
|
+
void dump(
|
422
|
+
unsigned int buf_sf1[10], unsigned int buf_sf2[10], unsigned int buf_sf3[10],
|
423
|
+
const GPS_Time<FloatT> &t){
|
424
|
+
typedef typename GPS_SpaceNode<FloatT>
|
425
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
426
|
+
GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
|
427
|
+
raw = *self;
|
428
|
+
unsigned int *buf[10] = {buf_sf1, buf_sf2, buf_sf3};
|
429
|
+
for(int i(0); i < 3; ++i){
|
430
|
+
dump_t::how_set(buf[i], t);
|
431
|
+
raw.dump<2, 0>(buf[i], i + 1);
|
432
|
+
}
|
433
|
+
}
|
434
|
+
int parse_almanac(const unsigned int buf[10]){
|
435
|
+
typedef GPS_SpaceNode<FloatT>::BroadcastedMessage<unsigned int, 30> parse_t;
|
436
|
+
switch(parse_t::subframe_id(buf)){
|
437
|
+
case 4:
|
438
|
+
case 5:
|
439
|
+
break;
|
440
|
+
default: return -1;
|
441
|
+
}
|
442
|
+
typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
|
443
|
+
almanac_t::raw_t raw;
|
444
|
+
switch(parse_t::data_id(buf)){
|
445
|
+
case 1: // GPS
|
446
|
+
raw.update<2, 0>(buf);
|
447
|
+
if((raw.svid < 1) || (raw.svid > 32)){return -1;}
|
448
|
+
break;
|
449
|
+
case 3: // QZSS
|
450
|
+
reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
|
451
|
+
.update<2, 0>(buf);
|
452
|
+
if((raw.svid < 193) || (raw.svid > 202)){return -1;}
|
453
|
+
break;
|
454
|
+
default:
|
455
|
+
return -1;
|
456
|
+
}
|
457
|
+
almanac_t almanac;
|
458
|
+
*self = (GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris)(almanac = raw);
|
459
|
+
return self->svid;
|
460
|
+
}
|
461
|
+
/**
|
462
|
+
* Return broadcasted raw data of almanac data.
|
463
|
+
* @param buf_brdc pointer to store raw data of subframe 4 or 5.
|
464
|
+
* Each 30bit (MSB 2 bits are padding) length word is stored in each successive address of the pointer.
|
465
|
+
* @param t GPS time at broadcasting
|
466
|
+
* @param qzss_subframe if 4 or 5, this ephemeris is treated as QZSS, otherwise GPS (default).
|
467
|
+
*/
|
468
|
+
void dump_almanac(
|
469
|
+
unsigned int buf_brdc[10], const GPS_Time<FloatT> &t,
|
470
|
+
const unsigned int &qzss_subframe = 0){
|
471
|
+
typedef typename GPS_SpaceNode<FloatT>
|
472
|
+
::BroadcastedMessage<unsigned int, 30> dump_t;
|
473
|
+
dump_t::how_set(buf_brdc, t);
|
474
|
+
typedef GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac almanac_t;
|
475
|
+
almanac_t almanac;
|
476
|
+
almanac_t::raw_t raw;
|
477
|
+
raw = (almanac = *self);
|
478
|
+
switch(qzss_subframe){
|
479
|
+
case 4:
|
480
|
+
case 5:
|
481
|
+
reinterpret_cast<QZSS_SpaceNode<FloatT>::SatelliteProperties::Almanac::raw_t &>(raw)
|
482
|
+
.dump<2, 0>(buf_brdc, qzss_subframe);
|
483
|
+
break;
|
484
|
+
default:
|
485
|
+
raw.dump<2, 0>(buf_brdc);
|
486
|
+
}
|
487
|
+
}
|
387
488
|
%typemap(out) constellation_res_t {
|
388
489
|
%append_output(SWIG_NewPointerObj((new System_XYZ<FloatT, WGS84>($1.position)),
|
389
490
|
$descriptor(System_XYZ<FloatT, WGS84> *), SWIG_POINTER_OWN));
|
@@ -402,6 +503,7 @@ struct GPS_Ephemeris : public GPS_SpaceNode<FloatT>::SatelliteProperties::Epheme
|
|
402
503
|
}
|
403
504
|
#if defined(SWIGRUBY)
|
404
505
|
%rename("consistent?") is_consistent;
|
506
|
+
%rename("valid?") is_valid;
|
405
507
|
#endif
|
406
508
|
}
|
407
509
|
|
@@ -444,6 +546,10 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
444
546
|
SBAS_Ephemeris() : SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris() {}
|
445
547
|
SBAS_Ephemeris(const typename SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris &eph)
|
446
548
|
: SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris(eph) {}
|
549
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
550
|
+
return SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::is_valid(t);
|
551
|
+
}
|
552
|
+
GPS_Time<FloatT> t_applicable() const {return GPS_Time<FloatT>(this->WN, this->t_0);}
|
447
553
|
};
|
448
554
|
%}
|
449
555
|
%extend SBAS_Ephemeris {
|
@@ -457,6 +563,18 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
457
563
|
MAKE_ACCESSOR(ddx, FloatT); MAKE_ACCESSOR(ddy, FloatT); MAKE_ACCESSOR(ddz, FloatT);
|
458
564
|
MAKE_ACCESSOR(a_Gf0, FloatT);
|
459
565
|
MAKE_ACCESSOR(a_Gf1, FloatT);
|
566
|
+
/**
|
567
|
+
* Return broadcasted raw data of SBAS ephemeris
|
568
|
+
* @param buf_brdc pointer to store raw data of Type 9 message.
|
569
|
+
* 250 of 256 bits are effective, LSB 6 bits of last word are padding.
|
570
|
+
* @param preamble_idx index of preamble, 0(0x53), 1(0x9A), or 2(0xC6)
|
571
|
+
*/
|
572
|
+
void dump(unsigned int buf_brdc[8], const unsigned int &preamble_idx = 0){
|
573
|
+
SBAS_SpaceNode<FloatT>::DataBlock::preamble_set2(buf_brdc, preamble_idx);
|
574
|
+
SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris::raw_t raw;
|
575
|
+
(raw = *self).dump(buf_brdc);
|
576
|
+
SBAS_SpaceNode<FloatT>::DataBlock::parity_set(buf_brdc);
|
577
|
+
}
|
460
578
|
typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
|
461
579
|
const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0,
|
462
580
|
const bool &with_velocity = true) const {
|
@@ -466,6 +584,9 @@ struct SBAS_Ephemeris : public SBAS_SpaceNode<FloatT>::SatelliteProperties::Ephe
|
|
466
584
|
pv.position, pv.velocity, self->clock_error(t_tx), self->clock_error_dot(t_tx)};
|
467
585
|
return res;
|
468
586
|
}
|
587
|
+
#if defined(SWIGRUBY)
|
588
|
+
%rename("valid?") is_valid;
|
589
|
+
#endif
|
469
590
|
}
|
470
591
|
|
471
592
|
%extend SBAS_SpaceNode {
|
@@ -519,6 +640,9 @@ struct GLONASS_Ephemeris
|
|
519
640
|
bool is_consistent() const {
|
520
641
|
return has_string == 0x1F;
|
521
642
|
}
|
643
|
+
bool is_valid(const GPS_Time<FloatT> &t) const {
|
644
|
+
return is_consistent() && eph_t::is_valid(t);
|
645
|
+
}
|
522
646
|
GLONASS_Ephemeris() : eph_t() {
|
523
647
|
invalidate();
|
524
648
|
}
|
@@ -607,6 +731,31 @@ struct GLONASS_Ephemeris
|
|
607
731
|
self->has_string = has_string;
|
608
732
|
return updated;
|
609
733
|
}
|
734
|
+
%apply unsigned int buf_brdc[ANY] {
|
735
|
+
unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
|
736
|
+
unsigned int buf_str4[3], unsigned int buf_str5[3]};
|
737
|
+
/**
|
738
|
+
* Return broadcasted raw data of GLONASS ephemeris data.
|
739
|
+
* @param buf_str1 pointer to store raw data of string 1.
|
740
|
+
* 85bit length data (LSB 11 bits are padding) is stored in successive address of the pointer.
|
741
|
+
* @param buf_str2 pointer to store raw data of string 2. Its structue is same as str1.
|
742
|
+
* @param buf_str3 pointer to store raw data of string 3. Its structue is same as str1.
|
743
|
+
* @param buf_str4 pointer to store raw data of string 4. Its structue is same as str1.
|
744
|
+
* @param buf_str5 pointer to store raw data of string 5. Its structue is same as str1.
|
745
|
+
* @param t GPS time at broadcasting
|
746
|
+
*/
|
747
|
+
void dump(
|
748
|
+
unsigned int buf_str1[3], unsigned int buf_str2[3], unsigned int buf_str3[3],
|
749
|
+
unsigned int buf_str4[3], unsigned int buf_str5[3],
|
750
|
+
const GPS_Time<FloatT> &t){
|
751
|
+
typename GLONASS_Ephemeris<FloatT>::eph_t::raw_t raw;
|
752
|
+
raw = *self;
|
753
|
+
unsigned int *buf[4] = {buf_str1, buf_str2, buf_str3, buf_str4};
|
754
|
+
for(int i(0); i < 4; ++i){
|
755
|
+
raw.GLONASS_Ephemeris<FloatT>::Ephemeris::raw_t::dump<0, 0>(buf[i], i + 1);
|
756
|
+
}
|
757
|
+
raw.GLONASS_Ephemeris<FloatT>::TimeProperties::raw_t::dump<0, 0>(buf_str5);
|
758
|
+
}
|
610
759
|
typename GPS_Ephemeris<FloatT>::constellation_res_t constellation(
|
611
760
|
const GPS_Time<FloatT> &t_tx, const FloatT &dt_transit = 0) const {
|
612
761
|
typename GPS_SpaceNode<FloatT>::SatelliteProperties::constellation_t pv(
|
@@ -617,6 +766,7 @@ struct GLONASS_Ephemeris
|
|
617
766
|
}
|
618
767
|
#if defined(SWIGRUBY)
|
619
768
|
%rename("consistent?") is_consistent;
|
769
|
+
%rename("valid?") is_valid;
|
620
770
|
%rename("in_range?") is_in_range;
|
621
771
|
#endif
|
622
772
|
bool is_in_range(const GPS_Time<FloatT> &t) const {
|
@@ -1081,6 +1231,43 @@ struct GPS_RangeCorrector
|
|
1081
1231
|
};
|
1082
1232
|
}
|
1083
1233
|
|
1234
|
+
%{
|
1235
|
+
template <class BaseT, class HookT>
|
1236
|
+
struct HookableSolver : public BaseT {
|
1237
|
+
typedef BaseT base_t;
|
1238
|
+
HookT *hook;
|
1239
|
+
HookableSolver(const BaseT &base) : BaseT(base), hook(NULL) {}
|
1240
|
+
virtual typename base_t::relative_property_t relative_property(
|
1241
|
+
const typename base_t::prn_t &prn,
|
1242
|
+
const typename base_t::measurement_t::mapped_type &measurement,
|
1243
|
+
const typename base_t::float_t &receiver_error,
|
1244
|
+
const typename base_t::gps_time_t &time_arrival,
|
1245
|
+
const typename base_t::pos_t &usr_pos,
|
1246
|
+
const typename base_t::xyz_t &usr_vel) const {
|
1247
|
+
typename base_t::relative_property_t res(
|
1248
|
+
base_t::relative_property(
|
1249
|
+
prn, measurement, receiver_error, time_arrival,
|
1250
|
+
usr_pos, usr_vel));
|
1251
|
+
if(hook){
|
1252
|
+
res = hook->relative_property(
|
1253
|
+
prn, measurement, receiver_error, time_arrival,
|
1254
|
+
usr_pos, usr_vel,
|
1255
|
+
res);
|
1256
|
+
}
|
1257
|
+
return res;
|
1258
|
+
}
|
1259
|
+
virtual typename base_t::satellite_t select_satellite(
|
1260
|
+
const typename base_t::prn_t &prn,
|
1261
|
+
const typename base_t::gps_time_t &time) const {
|
1262
|
+
typename base_t::satellite_t res(base_t::select_satellite(prn, time));
|
1263
|
+
if(hook){
|
1264
|
+
res = hook->select_satellite(prn, time, res);
|
1265
|
+
}
|
1266
|
+
return res;
|
1267
|
+
}
|
1268
|
+
};
|
1269
|
+
%}
|
1270
|
+
|
1084
1271
|
%extend GPS_Solver {
|
1085
1272
|
%ignore super_t;
|
1086
1273
|
%ignore base_t;
|
@@ -1090,7 +1277,7 @@ struct GPS_RangeCorrector
|
|
1090
1277
|
%ignore sbas;
|
1091
1278
|
%ignore glonass_t;
|
1092
1279
|
%ignore glonass;
|
1093
|
-
%ignore
|
1280
|
+
%ignore select;
|
1094
1281
|
%ignore relative_property;
|
1095
1282
|
%ignore select_satellite;
|
1096
1283
|
%ignore update_position_solution;
|
@@ -1110,14 +1297,12 @@ struct GPS_RangeCorrector
|
|
1110
1297
|
const GPS_Solver<FloatT>::base_t::float_t &receiver_error,
|
1111
1298
|
const GPS_Solver<FloatT>::base_t::gps_time_t &time_arrival,
|
1112
1299
|
const GPS_Solver<FloatT>::base_t::pos_t &usr_pos,
|
1113
|
-
const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel
|
1300
|
+
const GPS_Solver<FloatT>::base_t::xyz_t &usr_vel,
|
1301
|
+
const GPS_Solver<FloatT>::base_t::relative_property_t &res_orig) const {
|
1114
1302
|
union {
|
1115
1303
|
base_t::relative_property_t prop;
|
1116
1304
|
FloatT values[7];
|
1117
|
-
} res = {
|
1118
|
-
select_solver(prn).relative_property(
|
1119
|
-
prn, measurement, receiver_error, time_arrival,
|
1120
|
-
usr_pos, usr_vel)};
|
1305
|
+
} res = {res_orig};
|
1121
1306
|
#ifdef SWIGRUBY
|
1122
1307
|
do{
|
1123
1308
|
static const VALUE key(ID2SYM(rb_intern("relative_property")));
|
@@ -1197,9 +1382,9 @@ struct GPS_RangeCorrector
|
|
1197
1382
|
template <>
|
1198
1383
|
GPS_Solver<FloatT>::base_t::satellite_t GPS_Solver<FloatT>::select_satellite(
|
1199
1384
|
const GPS_Solver<FloatT>::base_t::prn_t &prn,
|
1200
|
-
const GPS_Solver<FloatT>::base_t::gps_time_t &time
|
1201
|
-
|
1202
|
-
|
1385
|
+
const GPS_Solver<FloatT>::base_t::gps_time_t &time,
|
1386
|
+
const GPS_Solver<FloatT>::base_t::satellite_t &res_orig) const {
|
1387
|
+
GPS_Solver<FloatT>::base_t::satellite_t res(res_orig);
|
1203
1388
|
#ifdef SWIGRUBY
|
1204
1389
|
if(!res.is_available()){
|
1205
1390
|
static const VALUE key(ID2SYM(rb_intern("relative_property")));
|
@@ -1396,7 +1581,7 @@ struct GPS_Solver
|
|
1396
1581
|
struct gps_t {
|
1397
1582
|
GPS_SpaceNode<FloatT> space_node;
|
1398
1583
|
GPS_SolverOptions<FloatT> options;
|
1399
|
-
GPS_SinglePositioning<FloatT> solver;
|
1584
|
+
HookableSolver<GPS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
|
1400
1585
|
struct ephemeris_proxy_t {
|
1401
1586
|
struct item_t {
|
1402
1587
|
const void *impl;
|
@@ -1418,19 +1603,19 @@ struct GPS_Solver
|
|
1418
1603
|
solver.satellites.impl_select = forward;
|
1419
1604
|
}
|
1420
1605
|
} ephemeris_proxy;
|
1421
|
-
gps_t() : space_node(), options(), solver(space_node), ephemeris_proxy(solver) {}
|
1606
|
+
gps_t() : space_node(), options(), solver(GPS_SinglePositioning<FloatT>(space_node)), ephemeris_proxy(solver) {}
|
1422
1607
|
} gps;
|
1423
1608
|
struct sbas_t {
|
1424
1609
|
SBAS_SpaceNode<FloatT> space_node;
|
1425
1610
|
SBAS_SolverOptions<FloatT> options;
|
1426
|
-
SBAS_SinglePositioning<FloatT> solver;
|
1427
|
-
sbas_t() : space_node(), options(), solver(space_node) {}
|
1611
|
+
HookableSolver<SBAS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
|
1612
|
+
sbas_t() : space_node(), options(), solver(SBAS_SinglePositioning<FloatT>(space_node)) {}
|
1428
1613
|
} sbas;
|
1429
1614
|
struct glonass_t {
|
1430
1615
|
GLONASS_SpaceNode<FloatT> space_node;
|
1431
1616
|
GLONASS_SolverOptions<FloatT> options;
|
1432
|
-
GLONASS_SinglePositioning<FloatT> solver;
|
1433
|
-
glonass_t() : space_node(), options(), solver(space_node) {}
|
1617
|
+
HookableSolver<GLONASS_SinglePositioning<FloatT>, GPS_Solver<FloatT> > solver;
|
1618
|
+
glonass_t() : space_node(), options(), solver(GLONASS_SinglePositioning<FloatT>(space_node)) {}
|
1434
1619
|
} glonass;
|
1435
1620
|
SWIG_Object hooks;
|
1436
1621
|
typedef std::vector<GPS_RangeCorrector<FloatT> > user_correctors_t;
|
@@ -1465,6 +1650,9 @@ struct GPS_Solver
|
|
1465
1650
|
= sbas.solver.tropospheric_correction
|
1466
1651
|
= glonass.solver.tropospheric_correction
|
1467
1652
|
= tropospheric;
|
1653
|
+
gps.solver.hook = this;
|
1654
|
+
sbas.solver.hook = this;
|
1655
|
+
glonass.solver.hook = this;
|
1468
1656
|
}
|
1469
1657
|
GPS_SpaceNode<FloatT> &gps_space_node() {return gps.space_node;}
|
1470
1658
|
GPS_SolverOptions<FloatT> &gps_options() {return gps.options;}
|
@@ -1472,28 +1660,44 @@ struct GPS_Solver
|
|
1472
1660
|
SBAS_SolverOptions<FloatT> &sbas_options() {return sbas.options;}
|
1473
1661
|
GLONASS_SpaceNode<FloatT> &glonass_space_node() {return glonass.space_node;}
|
1474
1662
|
GLONASS_SolverOptions<FloatT> &glonass_options() {return glonass.options;}
|
1475
|
-
const base_t &
|
1663
|
+
const base_t &select(
|
1476
1664
|
const typename base_t::prn_t &prn) const {
|
1477
1665
|
if(prn > 0 && prn <= 32){return gps.solver;}
|
1478
1666
|
if(prn >= 120 && prn <= 158){return sbas.solver;}
|
1479
1667
|
if(prn > 192 && prn <= 202){return gps.solver;}
|
1480
1668
|
if(prn > 0x100 && prn <= (0x100 + 24)){return glonass.solver;}
|
1481
|
-
|
1482
|
-
// => relative_property() => select_solver()
|
1483
|
-
// For not supported satellite, call loop prevention is required.
|
1484
|
-
static const base_t dummy;
|
1485
|
-
return dummy;
|
1669
|
+
return *this;
|
1486
1670
|
}
|
1671
|
+
// proxy of virtual functions
|
1672
|
+
typename base_t::relative_property_t relative_property(
|
1673
|
+
const typename base_t::prn_t &prn,
|
1674
|
+
const typename base_t::measurement_t::mapped_type &measurement,
|
1675
|
+
const typename base_t::float_t &receiver_error,
|
1676
|
+
const typename base_t::gps_time_t &time_arrival,
|
1677
|
+
const typename base_t::pos_t &usr_pos,
|
1678
|
+
const typename base_t::xyz_t &usr_vel,
|
1679
|
+
const typename base_t::relative_property_t &orig) const;
|
1487
1680
|
virtual typename base_t::relative_property_t relative_property(
|
1488
1681
|
const typename base_t::prn_t &prn,
|
1489
1682
|
const typename base_t::measurement_t::mapped_type &measurement,
|
1490
1683
|
const typename base_t::float_t &receiver_error,
|
1491
1684
|
const typename base_t::gps_time_t &time_arrival,
|
1492
1685
|
const typename base_t::pos_t &usr_pos,
|
1493
|
-
const typename base_t::xyz_t &usr_vel) const
|
1686
|
+
const typename base_t::xyz_t &usr_vel) const {
|
1687
|
+
return relative_property(
|
1688
|
+
prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel,
|
1689
|
+
super_t::relative_property(
|
1690
|
+
prn, measurement, receiver_error, time_arrival, usr_pos, usr_vel));
|
1691
|
+
}
|
1692
|
+
typename base_t::satellite_t select_satellite(
|
1693
|
+
const typename base_t::prn_t &prn,
|
1694
|
+
const typename base_t::gps_time_t &time,
|
1695
|
+
const typename base_t::satellite_t &orig) const;
|
1494
1696
|
virtual typename base_t::satellite_t select_satellite(
|
1495
1697
|
const typename base_t::prn_t &prn,
|
1496
|
-
const typename base_t::gps_time_t &time) const
|
1698
|
+
const typename base_t::gps_time_t &time) const {
|
1699
|
+
return select_satellite(prn, time, super_t::select_satellite(prn, time));
|
1700
|
+
}
|
1497
1701
|
virtual bool update_position_solution(
|
1498
1702
|
const typename base_t::geometric_matrices_t &geomat,
|
1499
1703
|
typename base_t::user_pvt_t &res) const;
|