gps_pvt 0.8.0 → 0.8.2
Sign up to get free protection for your applications and to get access to all the features.
- checksums.yaml +4 -4
- data/Rakefile +1 -0
- data/exe/gps_pvt +1 -17
- data/exe/to_ubx +162 -32
- data/ext/gps_pvt/GPS/GPS_wrap.cxx +1677 -399
- data/ext/ninja-scan-light/tool/navigation/GLONASS.h +137 -9
- data/ext/ninja-scan-light/tool/navigation/GLONASS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/navigation/GPS.h +301 -50
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/navigation/GPS_Solver_MultiFrequency.h +1 -0
- 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/navigation/SBAS_Solver.h +5 -3
- data/ext/ninja-scan-light/tool/param/bit_array.h +53 -16
- data/ext/ninja-scan-light/tool/swig/GPS.i +265 -25
- data/ext/ninja-scan-light/tool/swig/spec/GPS_spec.rb +64 -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 +49 -32
- data/lib/gps_pvt/version.rb +1 -1
- metadata +4 -2
@@ -51,6 +51,8 @@
|
|
51
51
|
|
52
52
|
#include "coordinate.h"
|
53
53
|
|
54
|
+
#include "util/bit_counter.h"
|
55
|
+
|
54
56
|
#ifdef pow2
|
55
57
|
#define POW2_ALREADY_DEFINED
|
56
58
|
#else
|
@@ -762,6 +764,44 @@ class GPS_SpaceNode {
|
|
762
764
|
InputT>(buf, index)
|
763
765
|
>> ((sizeof(OutputT) * CHAR_BIT) - length));
|
764
766
|
}
|
767
|
+
|
768
|
+
template <class NumberT, class BufferT>
|
769
|
+
static void num2bits(
|
770
|
+
BufferT *dest, NumberT src, const uint_t &index, uint_t length,
|
771
|
+
const int &effective_bits_in_BufferT = sizeof(BufferT) * CHAR_BIT,
|
772
|
+
const int &padding_bits_in_BufferT_MSB = 0){
|
773
|
+
static const int buf_bits(sizeof(BufferT) * CHAR_BIT);
|
774
|
+
const BufferT mask_msb_aligned( // "1.(effective).10..0"
|
775
|
+
(~(BufferT)0) << (buf_bits - effective_bits_in_BufferT));
|
776
|
+
const BufferT mask_full_n( // "1.(padding).10.(effective).01..1"
|
777
|
+
~(mask_msb_aligned >> padding_bits_in_BufferT_MSB));
|
778
|
+
BufferT buf, mask;
|
779
|
+
do{ // Least significant block
|
780
|
+
std::div_t align(std::div(index + length, effective_bits_in_BufferT));
|
781
|
+
dest += align.quot;
|
782
|
+
if(align.rem == 0){break;}
|
783
|
+
int len_last(align.rem);
|
784
|
+
if((int)length <= align.rem){len_last = length;}
|
785
|
+
int r_shift(padding_bits_in_BufferT_MSB + align.rem - len_last);
|
786
|
+
((buf = src) <<= (buf_bits - len_last)) >>= r_shift;
|
787
|
+
((mask = mask_msb_aligned) <<= (effective_bits_in_BufferT - len_last)) >>= r_shift;
|
788
|
+
(*dest &= ~mask) |= (buf & mask);
|
789
|
+
src >>= len_last;
|
790
|
+
length -= len_last;
|
791
|
+
}while(false);
|
792
|
+
std::div_t qr(std::div(length, effective_bits_in_BufferT));
|
793
|
+
for(; qr.quot > 0; qr.quot--, src >>= effective_bits_in_BufferT){ // Middle
|
794
|
+
((buf = src) <<= (buf_bits - effective_bits_in_BufferT))
|
795
|
+
>>= padding_bits_in_BufferT_MSB;
|
796
|
+
(*(--dest) &= mask_full_n) |= (buf & ~mask_full_n);
|
797
|
+
}
|
798
|
+
if(qr.rem > 0){ // Most
|
799
|
+
int r_shift(padding_bits_in_BufferT_MSB + effective_bits_in_BufferT - qr.rem);
|
800
|
+
((buf = src) <<= (buf_bits - qr.rem)) >>= r_shift;
|
801
|
+
mask = (mask_msb_aligned << (effective_bits_in_BufferT - qr.rem)) >> r_shift;
|
802
|
+
(*(--dest) &= ~mask) |= (buf & mask);
|
803
|
+
}
|
804
|
+
}
|
765
805
|
};
|
766
806
|
|
767
807
|
template <class InputT,
|
@@ -773,6 +813,10 @@ static u ## bits ## _t name(const InputT *buf){ \
|
|
773
813
|
return \
|
774
814
|
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
775
815
|
buf, offset_bits, length); \
|
816
|
+
} \
|
817
|
+
static void name ## _set(InputT *dest, const u ## bits ## _t &src){ \
|
818
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
819
|
+
dest, src, offset_bits, length, EffectiveBits, PaddingBits_MSB); \
|
776
820
|
}
|
777
821
|
#define convert_s(bits, offset_bits, length, name) \
|
778
822
|
static s ## bits ## _t name(const InputT *buf){ \
|
@@ -780,6 +824,10 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
780
824
|
DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
781
825
|
buf, offset_bits)) \
|
782
826
|
>> (bits - length); \
|
827
|
+
} \
|
828
|
+
static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
829
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
830
|
+
dest, *(u ## bits ## _t *)(&src), offset_bits, length, EffectiveBits, PaddingBits_MSB); \
|
783
831
|
}
|
784
832
|
#define convert_u_2(bits, offset_bits1, length1, offset_bits2, length2, name) \
|
785
833
|
static u ## bits ## _t name(const InputT *buf){ \
|
@@ -788,6 +836,12 @@ static u ## bits ## _t name(const InputT *buf){ \
|
|
788
836
|
buf, offset_bits1, length1) << length2) \
|
789
837
|
| DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
790
838
|
buf, offset_bits2, length2); \
|
839
|
+
} \
|
840
|
+
static void name ## _set(InputT *dest, const u ## bits ## _t &src){ \
|
841
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
842
|
+
dest, src >> length2, offset_bits1, length1, EffectiveBits, PaddingBits_MSB); \
|
843
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
844
|
+
dest, src, offset_bits2, length2, EffectiveBits, PaddingBits_MSB); \
|
791
845
|
}
|
792
846
|
#define convert_s_2(bits, offset_bits1, length1, offset_bits2, length2, name) \
|
793
847
|
static s ## bits ## _t name(const InputT *buf){ \
|
@@ -797,11 +851,75 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
797
851
|
| (DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
|
798
852
|
buf, offset_bits2, length2) << (bits - length1 - length2)))) \
|
799
853
|
>> (bits - length1 - length2); \
|
854
|
+
} \
|
855
|
+
static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
|
856
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
857
|
+
dest, *(u ## bits ## _t *)(&src) >> length2, offset_bits1, length1, EffectiveBits, PaddingBits_MSB); \
|
858
|
+
DataParser::template num2bits<u ## bits ## _t, InputT>( \
|
859
|
+
dest, *(u ## bits ## _t *)(&src), offset_bits2, length2, EffectiveBits, PaddingBits_MSB); \
|
800
860
|
}
|
801
861
|
convert_u( 8, 0, 8, preamble);
|
862
|
+
static void preamble_set(InputT *dest){preamble_set(dest, (u8_t)0x8B);}
|
802
863
|
convert_u(32, 30, 24, how);
|
864
|
+
static void how_set(InputT *dest, const gps_time_t &t){
|
865
|
+
how_set(dest, ((u32_t)(t.seconds / 1.5) & 0x1FFFF) << 7);
|
866
|
+
}
|
803
867
|
convert_u( 8, 49, 3, subframe_id);
|
804
868
|
|
869
|
+
/**
|
870
|
+
* update parity bits based on current buffer
|
871
|
+
* @param buf buffer
|
872
|
+
* @param word_idx word index whose range is [0, 9] (0 means "Word 1")
|
873
|
+
* @see GPS ICD Table 20-XIV
|
874
|
+
*/
|
875
|
+
static void parity_update(InputT *buf, const int &word_idx){
|
876
|
+
u32_t word(DataParser::template bits2num<u32_t, EffectiveBits, PaddingBits_MSB>(
|
877
|
+
buf, 30 * word_idx, 24));
|
878
|
+
u8_t parity;
|
879
|
+
parity = (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0xEC7CD2) & 0x1); parity <<= 1;
|
880
|
+
parity |= (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0x763E69) & 0x1); parity <<= 1;
|
881
|
+
parity |= (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0xBB1F34) & 0x1); parity <<= 1;
|
882
|
+
parity |= (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0x5D8F9A) & 0x1); parity <<= 1;
|
883
|
+
parity |= (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0xAEC7CD) & 0x1); parity <<= 1;
|
884
|
+
parity |= (u8_t)(BitCounter<u32_t>::count(word & (u32_t)0x2DEA27) & 0x1);
|
885
|
+
DataParser::template num2bits<u8_t, InputT>(
|
886
|
+
buf, parity, 30 * word_idx + 24, 6, EffectiveBits, PaddingBits_MSB);
|
887
|
+
}
|
888
|
+
|
889
|
+
/**
|
890
|
+
* Get word data for transmission
|
891
|
+
* @param buf buffer, whose parity is assumed to be already updated
|
892
|
+
* @param word_idx word index whose range is [0, 9] (0 means "Word 1")
|
893
|
+
* @param D29_star 29th bit of previous transmitted word
|
894
|
+
* @param D30_star 30th bit of previous transmitted word
|
895
|
+
* @return (u32_t) a word consisting of leading 2 padding bits and following 30 informative bits
|
896
|
+
* @see GPS ICD Table 20-XIV
|
897
|
+
*/
|
898
|
+
static u32_t word_for_transmission(
|
899
|
+
const InputT *buf, const int &word_idx,
|
900
|
+
const bool &D29_star = false, const bool &D30_star = false){
|
901
|
+
u32_t word(DataParser::template bits2num<u32_t, EffectiveBits, PaddingBits_MSB>(
|
902
|
+
buf, 30 * word_idx, 30));
|
903
|
+
u32_t mask(0);
|
904
|
+
if(D29_star){
|
905
|
+
mask |= 0x29;
|
906
|
+
}
|
907
|
+
if(D30_star){
|
908
|
+
mask |= ((u32_t)0xFFFFFF << 6);
|
909
|
+
mask |= 0x16;
|
910
|
+
}
|
911
|
+
u32_t res(word ^ mask);
|
912
|
+
if((word_idx == 1) || (word_idx == 9)){ // trailing 2 bits of HOW(word 2) and word 10 should be zeros
|
913
|
+
if(res & 0x02){ // bit 29 should be zero
|
914
|
+
res ^= (u8_t)0x43; // modify bit 24 & 29 & 30
|
915
|
+
}
|
916
|
+
if(res & 0x01){ // bit 30 should be zero
|
917
|
+
res ^= (u8_t)0x81; // modify bit 23 & 30
|
918
|
+
}
|
919
|
+
}
|
920
|
+
return res;
|
921
|
+
}
|
922
|
+
|
805
923
|
struct SubFrame1 {
|
806
924
|
convert_u(16, 60, 10, WN);
|
807
925
|
convert_u( 8, 72, 4, URA);
|
@@ -839,14 +957,15 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
839
957
|
convert_s(16, 278, 14, dot_i0);
|
840
958
|
};
|
841
959
|
|
960
|
+
convert_u( 8, 60, 2, data_id);
|
842
961
|
convert_u( 8, 62, 6, sv_page_id);
|
843
962
|
|
844
|
-
struct
|
963
|
+
struct SubFrame4_5_Almanac {
|
845
964
|
convert_u(16, 68, 16, e);
|
846
965
|
convert_u( 8, 90, 8, t_oa);
|
847
966
|
convert_s(16, 98, 16, delta_i);
|
848
967
|
convert_s(16, 120, 16, dot_Omega0);
|
849
|
-
convert_u( 8,
|
968
|
+
convert_u( 8, 136, 8, SV_health);
|
850
969
|
convert_u(32, 150, 24, sqrt_A);
|
851
970
|
convert_s(32, 180, 24, Omega0);
|
852
971
|
convert_s(32, 210, 24, omega);
|
@@ -913,19 +1032,38 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
913
1032
|
u8_t DN; ///< Last leap second update day (days)
|
914
1033
|
s8_t delta_t_LSF; ///< Updated leap seconds (s)
|
915
1034
|
|
916
|
-
#define fetch_item(name) name = BroadcastedMessage< \
|
917
|
-
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
|
918
|
-
:: SubFrame4_Page18 :: name (src)
|
919
1035
|
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
920
1036
|
void update(const InputT *src){
|
1037
|
+
typedef typename BroadcastedMessage<
|
1038
|
+
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
1039
|
+
::SubFrame4_Page18 parse_t;
|
1040
|
+
#define fetch_item(name) name = parse_t::name(src)
|
921
1041
|
fetch_item(alpha0); fetch_item(alpha1); fetch_item(alpha2); fetch_item(alpha3);
|
922
1042
|
fetch_item(beta0); fetch_item(beta1); fetch_item(beta2); fetch_item(beta3);
|
923
1043
|
fetch_item(A1); fetch_item(A0);
|
924
1044
|
fetch_item(WN_t); fetch_item(WN_LSF);
|
925
1045
|
fetch_item(t_ot); fetch_item(delta_t_LS); fetch_item(delta_t_LSF);
|
926
1046
|
fetch_item(DN);
|
927
|
-
}
|
928
1047
|
#undef fetch_item
|
1048
|
+
}
|
1049
|
+
|
1050
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
1051
|
+
void dump(BufferT *dst){
|
1052
|
+
typedef BroadcastedMessage<
|
1053
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
1054
|
+
deparse_t;
|
1055
|
+
deparse_t::preamble_set(dst);
|
1056
|
+
deparse_t::subframe_id_set(dst, 4);
|
1057
|
+
deparse_t::sv_page_id_set(dst, 56);
|
1058
|
+
#define dump_item(name) deparse_t::SubFrame4_Page18:: name ## _set(dst, name)
|
1059
|
+
dump_item(alpha0); dump_item(alpha1); dump_item(alpha2); dump_item(alpha3);
|
1060
|
+
dump_item(beta0); dump_item(beta1); dump_item(beta2); dump_item(beta3);
|
1061
|
+
dump_item(A1); dump_item(A0);
|
1062
|
+
dump_item(WN_t); dump_item(WN_LSF);
|
1063
|
+
dump_item(t_ot); dump_item(delta_t_LS); dump_item(delta_t_LSF);
|
1064
|
+
dump_item(DN);
|
1065
|
+
#undef dump_item
|
1066
|
+
}
|
929
1067
|
|
930
1068
|
enum {
|
931
1069
|
SF_alpha0,
|
@@ -945,10 +1083,9 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
945
1083
|
|
946
1084
|
operator Ionospheric_UTC_Parameters() const {
|
947
1085
|
Ionospheric_UTC_Parameters converted;
|
948
|
-
#define CONVERT(TARGET) \
|
949
|
-
{converted.TARGET = sf[SF_ ## TARGET] * TARGET;}
|
950
1086
|
#define CONVERT2(dst, src) \
|
951
1087
|
{converted.dst = sf[SF_ ## src] * src;}
|
1088
|
+
#define CONVERT(TARGET) CONVERT2(TARGET, TARGET)
|
952
1089
|
CONVERT2(alpha[0], alpha0);
|
953
1090
|
CONVERT2(alpha[1], alpha1);
|
954
1091
|
CONVERT2(alpha[2], alpha2);
|
@@ -969,6 +1106,31 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
969
1106
|
#undef CONVERT2
|
970
1107
|
return converted;
|
971
1108
|
};
|
1109
|
+
|
1110
|
+
raw_t &operator=(const Ionospheric_UTC_Parameters ¶ms) {
|
1111
|
+
#define CONVERT2(src, dst, type) \
|
1112
|
+
{dst = (type)std::floor(params.src / sf[SF_ ## dst] + 0.5);}
|
1113
|
+
#define CONVERT(TARGET, type) CONVERT2(TARGET, TARGET, type)
|
1114
|
+
CONVERT2(alpha[0], alpha0, s8_t);
|
1115
|
+
CONVERT2(alpha[1], alpha1, s8_t);
|
1116
|
+
CONVERT2(alpha[2], alpha2, s8_t);
|
1117
|
+
CONVERT2(alpha[3], alpha3, s8_t);
|
1118
|
+
CONVERT2(beta[0], beta0, s8_t);
|
1119
|
+
CONVERT2(beta[1], beta1, s8_t);
|
1120
|
+
CONVERT2(beta[2], beta2, s8_t);
|
1121
|
+
CONVERT2(beta[3], beta3, s8_t);
|
1122
|
+
CONVERT(A1, s32_t);
|
1123
|
+
CONVERT(A0, s32_t);
|
1124
|
+
t_ot = (u8_t)((t_ot >> 12) & 0xFF);
|
1125
|
+
WN_t = (u8_t)(params.WN_t & 0xFF);
|
1126
|
+
delta_t_LS = (s8_t)params.delta_t_LS;
|
1127
|
+
WN_LSF = (u8_t)(params.WN_LSF & 0xFF);
|
1128
|
+
DN = (u8_t)(params.DN & 0xFF);
|
1129
|
+
delta_t_LSF = (s8_t)params.delta_t_LSF;
|
1130
|
+
#undef CONVERT
|
1131
|
+
#undef CONVERT2
|
1132
|
+
return *this;
|
1133
|
+
}
|
972
1134
|
};
|
973
1135
|
};
|
974
1136
|
public:
|
@@ -1340,6 +1502,37 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1340
1502
|
return iode;
|
1341
1503
|
}
|
1342
1504
|
#undef fetch_item
|
1505
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
1506
|
+
void dump(BufferT *dst, const unsigned int &subframe){
|
1507
|
+
typedef BroadcastedMessage<
|
1508
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
1509
|
+
deparse_t;
|
1510
|
+
#define dump_item(num, name) deparse_t::SubFrame ## num :: name ## _set(dst, name)
|
1511
|
+
switch(subframe){
|
1512
|
+
case 1:
|
1513
|
+
dump_item(1, WN); dump_item(1, URA); dump_item(1, SV_health);
|
1514
|
+
dump_item(1, iodc); dump_item(1, t_GD); dump_item(1, t_oc);
|
1515
|
+
dump_item(1, a_f2); dump_item(1, a_f1); dump_item(1, a_f0);
|
1516
|
+
break;
|
1517
|
+
case 2: {
|
1518
|
+
dump_item(2, iode); dump_item(2, c_rs); dump_item(2, delta_n);
|
1519
|
+
dump_item(2, M0); dump_item(2, c_uc); dump_item(2, e);
|
1520
|
+
dump_item(2, c_us); dump_item(2, sqrt_A); dump_item(2, t_oe);
|
1521
|
+
u8_t fit(fit_interval_flag ? 1 : 0); dump_item(2, fit);
|
1522
|
+
break;
|
1523
|
+
}
|
1524
|
+
case 3:
|
1525
|
+
dump_item(3, c_ic); dump_item(3, Omega0); dump_item(3, c_is);
|
1526
|
+
dump_item(3, i0); dump_item(3, c_rc); dump_item(3, omega);
|
1527
|
+
dump_item(3, dot_Omega0); dump_item(3, dot_i0); dump_item(3, iode);
|
1528
|
+
break;
|
1529
|
+
default:
|
1530
|
+
return;
|
1531
|
+
}
|
1532
|
+
#undef dump_item
|
1533
|
+
deparse_t::preamble_set(dst);
|
1534
|
+
deparse_t::subframe_id_set(dst, subframe);
|
1535
|
+
}
|
1343
1536
|
|
1344
1537
|
static float_t fit_interval(const bool &_flag, const u16_t &_iodc){
|
1345
1538
|
// Fit interval (ICD:20.3.4.4)
|
@@ -1440,38 +1633,38 @@ static s ## bits ## _t name(const InputT *buf){ \
|
|
1440
1633
|
}
|
1441
1634
|
|
1442
1635
|
raw_t &operator=(const Ephemeris &eph){
|
1443
|
-
#define CONVERT(TARGET) \
|
1444
|
-
{TARGET = (
|
1636
|
+
#define CONVERT(type, TARGET) \
|
1637
|
+
{TARGET = (type)std::floor(eph.TARGET / sf[SF_ ## TARGET] + 0.5);}
|
1445
1638
|
svid = eph.svid;
|
1446
1639
|
|
1447
1640
|
WN = eph.WN;
|
1448
1641
|
URA = URA_index(eph.URA);
|
1449
1642
|
SV_health = eph.SV_health;
|
1450
1643
|
iodc = eph.iodc;
|
1451
|
-
CONVERT(t_GD);
|
1452
|
-
CONVERT(t_oc);
|
1453
|
-
CONVERT(a_f0);
|
1454
|
-
CONVERT(a_f1);
|
1455
|
-
CONVERT(a_f2);
|
1644
|
+
CONVERT(s8_t, t_GD);
|
1645
|
+
CONVERT(u16_t, t_oc);
|
1646
|
+
CONVERT(s32_t, a_f0);
|
1647
|
+
CONVERT(s16_t, a_f1);
|
1648
|
+
CONVERT(s8_t, a_f2);
|
1456
1649
|
|
1457
1650
|
iode = eph.iode;
|
1458
|
-
CONVERT(c_rs);
|
1459
|
-
CONVERT(delta_n);
|
1460
|
-
CONVERT(M0);
|
1461
|
-
CONVERT(c_uc);
|
1462
|
-
CONVERT(e);
|
1463
|
-
CONVERT(c_us);
|
1464
|
-
CONVERT(sqrt_A);
|
1465
|
-
CONVERT(t_oe);
|
1466
|
-
|
1467
|
-
CONVERT(c_ic);
|
1468
|
-
CONVERT(Omega0);
|
1469
|
-
CONVERT(c_is);
|
1470
|
-
CONVERT(i0);
|
1471
|
-
CONVERT(c_rc);
|
1472
|
-
CONVERT(omega);
|
1473
|
-
CONVERT(dot_Omega0);
|
1474
|
-
CONVERT(dot_i0);
|
1651
|
+
CONVERT(s16_t, c_rs);
|
1652
|
+
CONVERT(s16_t, delta_n);
|
1653
|
+
CONVERT(s32_t, M0);
|
1654
|
+
CONVERT(s16_t, c_uc);
|
1655
|
+
CONVERT(u32_t, e);
|
1656
|
+
CONVERT(s16_t, c_us);
|
1657
|
+
CONVERT(u32_t, sqrt_A);
|
1658
|
+
CONVERT(u16_t, t_oe);
|
1659
|
+
|
1660
|
+
CONVERT(s16_t, c_ic);
|
1661
|
+
CONVERT(s32_t, Omega0);
|
1662
|
+
CONVERT(s16_t, c_is);
|
1663
|
+
CONVERT(s32_t, i0);
|
1664
|
+
CONVERT(s16_t, c_rc);
|
1665
|
+
CONVERT(s32_t, omega);
|
1666
|
+
CONVERT(s32_t, dot_Omega0);
|
1667
|
+
CONVERT(s16_t, dot_i0);
|
1475
1668
|
#undef CONVERT
|
1476
1669
|
fit_interval_flag = (eph.fit_interval > 5 * 60 * 60);
|
1477
1670
|
|
@@ -1542,7 +1735,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1542
1735
|
float_t a_f1; ///< Clock correction parameter (s)
|
1543
1736
|
|
1544
1737
|
/**
|
1545
|
-
*
|
1738
|
+
* Upgrade to ephemeris
|
1546
1739
|
*
|
1547
1740
|
*/
|
1548
1741
|
operator Ephemeris() const {
|
@@ -1577,7 +1770,7 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1577
1770
|
converted.c_ic = 0; // Cosine correction, inclination (rad)
|
1578
1771
|
converted.Omega0 = Omega0; // Longitude of ascending node (rad)
|
1579
1772
|
converted.c_is = 0; // Sine correction, inclination (rad)
|
1580
|
-
converted.i0 = delta_i;
|
1773
|
+
converted.i0 = delta_i + SC2RAD * 0.3; // Inclination angle (rad)
|
1581
1774
|
converted.c_rc = 0; // Cosine correction, orbit (m)
|
1582
1775
|
converted.omega = omega; // Argument of perigee (rad)
|
1583
1776
|
converted.dot_Omega0 = dot_Omega0; // Rate of right ascension (rad/s)
|
@@ -1585,6 +1778,20 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1585
1778
|
|
1586
1779
|
return converted;
|
1587
1780
|
}
|
1781
|
+
/**
|
1782
|
+
* downgrade from ephemeris
|
1783
|
+
*/
|
1784
|
+
Almanac &operator=(const Ephemeris &eph) {
|
1785
|
+
#define copy_item(dst, src) dst = eph.src
|
1786
|
+
copy_item(svid, svid); copy_item(e, e); copy_item(t_oa, t_oe);
|
1787
|
+
copy_item(delta_i, i0); copy_item(dot_Omega0, dot_Omega0);
|
1788
|
+
copy_item(SV_health, SV_health); copy_item(sqrt_A, sqrt_A);
|
1789
|
+
copy_item(Omega0, Omega0); copy_item(omega, omega); copy_item(M0, M0);
|
1790
|
+
copy_item(a_f0, a_f0); copy_item(a_f1, a_f1);
|
1791
|
+
#undef copy_item
|
1792
|
+
delta_i -= SC2RAD * 0.3;
|
1793
|
+
return *this;
|
1794
|
+
}
|
1588
1795
|
|
1589
1796
|
struct raw_t {
|
1590
1797
|
u8_t svid; ///< Satellite number
|
@@ -1601,11 +1808,13 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1601
1808
|
s16_t a_f0; ///< Clock corr. param. (-20, s)
|
1602
1809
|
s16_t a_f1; ///< Clock corr. param. (-38, s)
|
1603
1810
|
|
1604
|
-
#define fetch_item(name) name = BroadcastedMessage< \
|
1605
|
-
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
|
1606
|
-
:: SubFrame4_5_Alnamac :: name (src)
|
1607
1811
|
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
1608
1812
|
void update(const InputT *src){
|
1813
|
+
typedef BroadcastedMessage<
|
1814
|
+
InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
1815
|
+
parse_t;
|
1816
|
+
#define fetch_item(name) name = parse_t::SubFrame4_5_Almanac:: name (src)
|
1817
|
+
svid = parse_t::sv_page_id(src);
|
1609
1818
|
fetch_item(e);
|
1610
1819
|
fetch_item(t_oa);
|
1611
1820
|
fetch_item(delta_i);
|
@@ -1617,8 +1826,31 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1617
1826
|
fetch_item(M0);
|
1618
1827
|
fetch_item(a_f0);
|
1619
1828
|
fetch_item(a_f1);
|
1620
|
-
}
|
1621
1829
|
#undef fetch_item
|
1830
|
+
}
|
1831
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
1832
|
+
void dump(BufferT *dst){
|
1833
|
+
typedef BroadcastedMessage<
|
1834
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
1835
|
+
deparse_t;
|
1836
|
+
#define dump_item(name) deparse_t::SubFrame4_5_Almanac:: name ## _set(dst, name)
|
1837
|
+
dump_item(e); dump_item(t_oa); dump_item(delta_i);
|
1838
|
+
dump_item(dot_Omega0); dump_item(SV_health); dump_item(sqrt_A);
|
1839
|
+
dump_item(Omega0); dump_item(omega); dump_item(M0);
|
1840
|
+
dump_item(a_f0); dump_item(a_f1);
|
1841
|
+
#undef dump_item
|
1842
|
+
deparse_t::preamble_set(dst);
|
1843
|
+
if((svid >= 1) && (svid <= 24)){
|
1844
|
+
deparse_t::subframe_id_set(dst, 5);
|
1845
|
+
deparse_t::sv_page_id_set(dst, svid);
|
1846
|
+
}else if((svid >= 25) && (svid <= 32)){ // subframe 4 and page 2-5, 7-10
|
1847
|
+
deparse_t::subframe_id_set(dst, 4);
|
1848
|
+
deparse_t::sv_page_id_set(dst, svid);
|
1849
|
+
}else{ // provision for other systems (ex. QZSS)
|
1850
|
+
return;
|
1851
|
+
}
|
1852
|
+
deparse_t::data_id_set(dst, 1); // always "01" @see 20.3.3.5.1.1 Data ID and SV ID.
|
1853
|
+
}
|
1622
1854
|
|
1623
1855
|
enum {
|
1624
1856
|
SF_e,
|
@@ -1640,21 +1872,40 @@ if(std::abs(TARGET - eph.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
|
|
1640
1872
|
Almanac converted;
|
1641
1873
|
#define CONVERT(TARGET) \
|
1642
1874
|
{converted.TARGET = sf[SF_ ## TARGET] * TARGET;}
|
1643
|
-
|
1644
|
-
|
1645
|
-
|
1646
|
-
|
1647
|
-
|
1648
|
-
|
1649
|
-
|
1650
|
-
|
1651
|
-
|
1652
|
-
|
1653
|
-
|
1654
|
-
|
1875
|
+
converted.svid = svid;
|
1876
|
+
CONVERT(e);
|
1877
|
+
CONVERT(t_oa);
|
1878
|
+
CONVERT(delta_i);
|
1879
|
+
CONVERT(dot_Omega0);
|
1880
|
+
converted.SV_health = SV_health;
|
1881
|
+
CONVERT(sqrt_A);
|
1882
|
+
CONVERT(Omega0);
|
1883
|
+
CONVERT(omega);
|
1884
|
+
CONVERT(M0);
|
1885
|
+
CONVERT(a_f0);
|
1886
|
+
CONVERT(a_f1);
|
1655
1887
|
#undef CONVERT
|
1656
1888
|
return converted;
|
1657
1889
|
}
|
1890
|
+
|
1891
|
+
raw_t &operator=(const Almanac &alm) {
|
1892
|
+
#define CONVERT(type, key) \
|
1893
|
+
{key = (type)std::floor(alm.key / sf[SF_ ## key] + 0.5);}
|
1894
|
+
svid = (u8_t)alm.svid;
|
1895
|
+
CONVERT(u16_t, e);
|
1896
|
+
CONVERT(u8_t, t_oa);
|
1897
|
+
CONVERT(s16_t, delta_i);
|
1898
|
+
CONVERT(s16_t, dot_Omega0);
|
1899
|
+
SV_health = (u8_t)alm.SV_health;
|
1900
|
+
CONVERT(u32_t, sqrt_A);
|
1901
|
+
CONVERT(u32_t, Omega0);
|
1902
|
+
CONVERT(u32_t, omega);
|
1903
|
+
CONVERT(u32_t, M0);
|
1904
|
+
CONVERT(s16_t, a_f0);
|
1905
|
+
CONVERT(s16_t, a_f1);
|
1906
|
+
#undef CONVERT
|
1907
|
+
return *this;
|
1908
|
+
}
|
1658
1909
|
};
|
1659
1910
|
};
|
1660
1911
|
};
|
@@ -77,12 +77,14 @@ struct GPS_SinglePositioning_Options : public GPS_Solver_GeneralOptions<FloatT>
|
|
77
77
|
|
78
78
|
template <class FloatT, class SolverBaseT = GPS_Solver_Base<FloatT> >
|
79
79
|
class GPS_SinglePositioning : public SolverBaseT {
|
80
|
-
private:
|
81
|
-
GPS_SinglePositioning<FloatT> &operator=(const GPS_SinglePositioning<FloatT> &);
|
82
80
|
public:
|
83
|
-
typedef GPS_SinglePositioning<FloatT> self_t;
|
81
|
+
typedef GPS_SinglePositioning<FloatT, SolverBaseT> self_t;
|
84
82
|
typedef SolverBaseT base_t;
|
83
|
+
private:
|
84
|
+
self_t &operator=(const self_t &);
|
85
|
+
GPS_SinglePositioning(const self_t &);
|
85
86
|
|
87
|
+
public:
|
86
88
|
#if defined(__GNUC__) && (__GNUC__ < 5)
|
87
89
|
#define inheritate_type(x) typedef typename base_t::x x;
|
88
90
|
#else
|
@@ -52,6 +52,7 @@ class GPS_Solver_MultiFrequency : public BaseSolver {
|
|
52
52
|
typedef BaseSolver super_t;
|
53
53
|
private:
|
54
54
|
self_t &operator=(const self_t &);
|
55
|
+
GPS_Solver_MultiFrequency(const self_t &);
|
55
56
|
public:
|
56
57
|
#if defined(__GNUC__) && (__GNUC__ < 5)
|
57
58
|
#define inheritate_type(x) typedef typename super_t::x x;
|
@@ -40,23 +40,56 @@
|
|
40
40
|
#include <limits>
|
41
41
|
|
42
42
|
template <class FloatT>
|
43
|
-
struct
|
44
|
-
|
45
|
-
|
43
|
+
struct QZSS_SpaceNode {
|
44
|
+
struct SatelliteProperties {
|
45
|
+
struct Ephemeris {
|
46
|
+
typedef typename GPS_SpaceNode<FloatT>::SatelliteProperties::Ephemeris eph_gps_t;
|
47
|
+
struct raw_t : public eph_gps_t::raw_t {
|
48
|
+
typedef typename eph_gps_t::raw_t super_t;
|
46
49
|
|
47
|
-
|
48
|
-
|
49
|
-
|
50
|
-
|
51
|
-
|
52
|
-
|
53
|
-
|
50
|
+
operator eph_gps_t() const {
|
51
|
+
eph_gps_t res(super_t::operator eph_gps_t());
|
52
|
+
res.fit_interval = super_t::fit_interval_flag
|
53
|
+
? (std::numeric_limits<FloatT>::max)()
|
54
|
+
: (2 * 60 * 60); // Fit interval (ICD:4.1.2.4 Subframe 2); should be zero
|
55
|
+
return res;
|
56
|
+
};
|
54
57
|
|
55
|
-
|
56
|
-
|
57
|
-
|
58
|
-
|
59
|
-
|
58
|
+
raw_t &operator=(const eph_gps_t &eph){
|
59
|
+
super_t::operator=(eph);
|
60
|
+
super_t::fit_interval_flag = (eph.fit_interval > 2 * 60 * 60);
|
61
|
+
return *this;
|
62
|
+
}
|
63
|
+
};
|
64
|
+
};
|
65
|
+
struct Almanac {
|
66
|
+
typedef typename GPS_SpaceNode<FloatT>::SatelliteProperties::Almanac alm_gps_t;
|
67
|
+
struct raw_t : public alm_gps_t::raw_t {
|
68
|
+
typedef typename alm_gps_t::raw_t super_t;
|
69
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
|
70
|
+
void update(const InputT *src){
|
71
|
+
super_t::template update<PaddingBits_MSB, PaddingBits_LSB, InputT>(src);
|
72
|
+
if((super_t::svid >= 1) && (super_t::svid <= 10)){
|
73
|
+
super_t::svid += 192;
|
74
|
+
}
|
75
|
+
}
|
76
|
+
template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
|
77
|
+
void dump(BufferT *dst, const typename GPS_SpaceNode<FloatT>::u8_t &sf = 4){
|
78
|
+
super_t::template dump<PaddingBits_MSB, PaddingBits_LSB, BufferT>(dst);
|
79
|
+
typedef typename GPS_SpaceNode<FloatT>::template BroadcastedMessage<
|
80
|
+
BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
|
81
|
+
deparse_t;
|
82
|
+
if((super_t::svid >= 193) && (super_t::svid <= 202)){
|
83
|
+
deparse_t::subframe_id_set(dst, sf); // sf = 4 or 5
|
84
|
+
deparse_t::sv_page_id_set(dst, super_t::svid - 192); // [1, 10]
|
85
|
+
}else{
|
86
|
+
return;
|
87
|
+
}
|
88
|
+
deparse_t::data_id_set(dst, 3); // always 3 @see 4.1.2.6.2. QZS Almanac
|
89
|
+
}
|
90
|
+
};
|
91
|
+
};
|
92
|
+
};
|
60
93
|
};
|
61
94
|
|
62
95
|
#endif /* __QZSS_H__ */
|