gps_pvt 0.8.0 → 0.8.1

data/ext/ninja-scan-light/tool/navigation/GLONASS.h

@@ -46,6 +46,7 @@
 
 #include "GPS.h"
 #include "algorithm/integral.h"
+#include "util/bit_counter.h"
 
 template <class FloatT = double>
 class GLONASS_SpaceNode {
@@ -89,17 +90,79 @@ static u ## bits ## _t name(const InputT *buf){ \
   return \
       DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
         buf, offset_bits, length); \
+} \
+static void name ## _set(InputT *dest, const u ## bits ## _t &src){ \
+  DataParser::template num2bits<u ## bits ## _t, InputT>( \
+      dest, src, offset_bits, length, EffectiveBits, PaddingBits_MSB); \
 }
 #define convert_s(bits, offset_bits, length, name) \
 static s ## bits ## _t name(const InputT *buf){ \
   u ## bits ## _t temp( \
       DataParser::template bits2num<u ## bits ## _t, EffectiveBits, PaddingBits_MSB>( \
         buf, offset_bits, length)); \
-  static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); /* MSB is sign bit */ \
+  /* MSB is sign bit (Not equal to two's complement?) */ \
+  static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \
   return (temp & mask) ? ((s ## bits ## _t)-1 * (temp & (mask - 1))) : temp; \
+} \
+static void name ## _set(InputT *dest, const s ## bits ## _t &src){ \
+  static const u ## bits ## _t mask((u ## bits ## _t)1 << (length - 1)); \
+  u ## bits ## _t src2((src < 0) ? ((-src) | mask) : src); \
+  DataParser::template num2bits<u ## bits ## _t, InputT>( \
+      dest, src2, offset_bits, length, EffectiveBits, PaddingBits_MSB); \
 }
+      convert_u( 8,  0,  1, idle);
+      static void idle_set(InputT *dest){idle_set(dest, 0);}
       convert_u( 8,  1,  4, m);
       convert_u( 8, 77,  8, KX);
+      static void KX_set(InputT *dest){
+#if 0
+        // mask generation with Ruby
+        mask_bits = 32
+        mask_str = "0x%0#{mask_bits / 4}X"
+        [
+          [9, 10, 12, 13, 15, 17, 19, 20, 22, 24, 26, 28, 30, 32, 34, 35, 37, 39, 41, 43,
+              45, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84],
+          [9, 11, 12, 14, 15, 18, 19, 21, 22, 25, 26, 29, 30, 33, 34, 36, 37, 40, 41, 44,
+              45, 48, 49, 52, 53, 56, 57, 60, 61, 64, 65, 67, 68, 71, 72, 75, 76, 79, 80, 83, 84],
+          [10..12, 16..19, 23..26, 31..34, 38..41, 46..49, 54..57, 62..65, 69..72, 77..80, 85],
+          [13..19, 27..34, 42..49, 58..65, 73..80],
+          [20..34, 50..65, 81..85],
+          [35..65],
+          [66..85],
+        ].collect{|pat|
+          pat.collect{|idx| idx.to_a rescue idx}.flatten \
+              .collect{|i| 85 - i}.sort.chunk{|i| i / mask_bits} \
+              .collect{|quot, i_s|
+                mask = eval("0b#{i_s.inject("0" * mask_bits){|res, i| res[i % mask_bits] = '1'; res}}")
+                #mask = [mask_be].pack("N").unpack("L")[0] # endianess correction
+                [quot, mask] # endianess correction
+              }.inject([mask_str % [0]] * (77.0 / mask_bits).ceil){|res, (i, mask)|
+                res[i] = (mask_str % [mask]); res
+              }
+        }.transpose
+#endif
+        u8_t hamming(0);
+        static const struct {
+          uint_t idx, len;
+          u32_t mask[8];
+        } prop[] = {
+          { 0, 32, {0x55555AAAu, 0x66666CCCu, 0x87878F0Fu, 0x07F80FF0u, 0xF8000FFFu, 0x00000FFFu, 0xFFFFF000u, ~0u}},
+          {32, 32, {0xAAAAB555u, 0xCCCCD999u, 0x0F0F1E1Eu, 0x0FF01FE0u, 0xF0001FFFu, 0xFFFFE000u, 0, ~0u}},
+          {64, 13, {0x6AD80000u >> 19, 0xB3680000u >> 19, 0x3C700000u >> 19, 0x3F800000u >> 19, 0xC0000000u >> 19, 0, 0, ~0u}},
+        };
+        for(std::size_t j(0); j < sizeof(prop) / sizeof(prop[0]); ++j){
+          u8_t check_bit(1);
+          u32_t v(DataParser::template bits2num<u32_t, EffectiveBits, PaddingBits_MSB>(dest, prop[j].idx, prop[j].len));
+          for(int i(0); i < 8; ++i, check_bit <<= 1){
+            if(BitCounter<u32_t>::count(v & prop[j].mask[i]) % 2 == 0){continue;}
+            hamming ^= check_bit;
+          }
+        }
+        if(BitCounter<u8_t>::count(hamming & 0x7F) % 2 == 1){
+          hamming ^= 0x80;
+        }
+        KX_set(dest, hamming);
+      }
 
       struct String1 {
         convert_u( 8,  7,  2, P1);
@@ -135,7 +198,7 @@ static s ## bits ## _t name(const InputT *buf){ \
         convert_u( 8, 70,  5, n);
         convert_u( 8, 75,  2, M);
       };
-      struct String5_Alnamac {
+      struct String5_Almanac {
         convert_u(16,  5, 11, NA);
         convert_s(32, 16, 32, tau_c);
         convert_u( 8, 49,  5, N_4);
@@ -143,7 +206,7 @@ static s ## bits ## _t name(const InputT *buf){ \
         convert_u( 8, 76,  1, l_n);
       };
 
-      struct String6_Alnamac { // String 6; same as 8, 10, 12, 14
+      struct String6_Almanac { // String 6; same as 8, 10, 12, 14
         convert_u( 8,  5,  1, C_n);
         convert_u( 8,  6,  2, M_n);
         convert_u( 8,  8,  5, nA);
@@ -152,7 +215,7 @@ static s ## bits ## _t name(const InputT *buf){ \
         convert_s(32, 44, 18, delta_iA_n);
         convert_u(16, 62, 15, epsilonA_n);
       };
-      struct String7_Alnamac { // String 7; same as 9, 11, 13, 15
+      struct String7_Almanac { // String 7; same as 9, 11, 13, 15
         convert_s(16,  5, 16, omegaA_n);
         convert_u(32, 21, 21, tA_lambda_n);
         convert_s(32, 42, 22, delta_TA_n);
@@ -257,7 +320,7 @@ static s ## bits ## _t name(const InputT *buf){ \
         bool l_n;
 #define fetch_item(name) name = BroadcastedMessage< \
    InputT, (int)sizeof(InputT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB> \
-   :: String5_Alnamac :: name (src)
+   :: String5_Almanac :: name (src)
         template <int PaddingBits_MSB, int PaddingBits_LSB, class InputT>
         void update_string5(const InputT *src){
           fetch_item(tau_c);
@@ -267,6 +330,22 @@ static s ## bits ## _t name(const InputT *buf){ \
           fetch_item(l_n);
         }
 #undef fetch_item
+        template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
+        void dump(BufferT *dst){
+          typedef BroadcastedMessage<
+              BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
+              deparse_t;
+#define dump_item(name) deparse_t::String5_Almanac:: name ## _set(dst, name)
+          dump_item(tau_c);
+          dump_item(tau_GPS);
+          dump_item(N_4);
+          dump_item(NA);
+          dump_item(l_n);
+#undef dump_item
+          deparse_t::m_set(dst, 5);
+          deparse_t::idle_set(dst);
+          deparse_t::KX_set(dst);
+        }
 
         enum {
           SF_tau_c, SF_tau_GPS,
@@ -307,7 +386,7 @@ static s ## bits ## _t name(const InputT *buf){ \
 
         raw_t &operator=(const TimeProperties &t){
 #define CONVERT(TARGET) \
-{TARGET = (s32_t)((t.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);}
+{TARGET = (s32_t)std::floor(t.TARGET / sf[SF_ ## TARGET] + 0.5);}
           CONVERT(tau_c);
           CONVERT(tau_GPS);
           std::div_t divmod(std::div(t.date.year - 1996, 4));
@@ -364,7 +443,7 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
           uint_t E_n; // [days]
           uint_t P1; // [s] time interval of two adjacent t_b; 0 - 0, 1 - 30, 2 - 45, 3 - 60 mins
           bool P2;
-          //bool P3; // flag for alnamac; 1 - five, 0 - four
+          //bool P3; // flag for almanac; 1 - five, 0 - four
           bool P4; // flag for ephemeris; 1 - uploaded by the control segment
           bool l_n; // health flag; 0 - healthy, 1 - malfunction
 
@@ -798,6 +877,56 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
               fetch_item(4, M);
             }
 #undef fetch_item
+            template <int PaddingBits_MSB, int PaddingBits_LSB, class BufferT>
+            void dump(BufferT *dst, const unsigned int &str){
+              typedef BroadcastedMessage<
+                  BufferT, (int)sizeof(BufferT) * CHAR_BIT - PaddingBits_MSB - PaddingBits_LSB, PaddingBits_MSB>
+                  deparse_t;
+#define dump_item(num, name) deparse_t::String ## num :: name ## _set(dst, name)
+              switch(str){
+                case 1:
+                  dump_item(1, P1);
+                  dump_item(1, t_k);
+                  dump_item(1, xn_dot);
+                  dump_item(1, xn_ddot);
+                  dump_item(1, xn);
+                  break;
+                case 2: {
+                  dump_item(2, B_n);
+                  dump_item(2, P2);
+                  dump_item(2, t_b);
+                  dump_item(2, yn_dot);
+                  dump_item(2, yn_ddot);
+                  dump_item(2, yn);
+                  break;
+                }
+                case 3:
+                  dump_item(3, P3);
+                  dump_item(3, gamma_n);
+                  dump_item(3, p);
+                  dump_item(3, l_n);
+                  dump_item(3, zn_dot);
+                  dump_item(3, zn_ddot);
+                  dump_item(3, zn);
+                  break;
+                case 4:
+                  dump_item(4, tau_n);
+                  dump_item(4, delta_tau_n);
+                  dump_item(4, E_n);
+                  dump_item(4, P4);
+                  dump_item(4, F_T);
+                  dump_item(4, N_T);
+                  dump_item(4, n);
+                  dump_item(4, M);
+                  break;
+                default:
+                  return;
+              }
+#undef dump_item
+              deparse_t::m_set(dst, str);
+              deparse_t::idle_set(dst);
+              deparse_t::KX_set(dst);
+            }
 
             enum {
               SF_xn,       SF_yn = SF_xn,            SF_zn = SF_xn,
@@ -859,9 +988,8 @@ if(std::abs(TARGET - t.TARGET) > raw_t::sf[raw_t::SF_ ## TARGET]){break;}
               return res;
             }
             raw_t &operator=(const Ephemeris &eph){
-              // TODO: m?
 #define CONVERT(TARGET) \
-{TARGET = (s32_t)((eph.TARGET + 0.5 * sf[SF_ ## TARGET]) / sf[SF_ ## TARGET]);}
+{TARGET = (s32_t)std::floor(eph.TARGET / sf[SF_ ## TARGET] + 0.5);}
               svid = eph.svid;
               { // t_k
                 std::div_t minutes(div(eph.t_k, 60));