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
@@ -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
|
};
|
@@ -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__ */
|