gps_pvt 0.8.0 → 0.8.1
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/Rakefile +1 -0
- data/exe/gps_pvt +1 -17
- 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/lib/gps_pvt/receiver/extension.rb +51 -0
- data/lib/gps_pvt/receiver.rb +13 -8
- data/lib/gps_pvt/version.rb +1 -1
- metadata +4 -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;
|