Add error detection functionality for GLO nav message

Author: Dmitry Tatarinov

include/libswiftnav/nav_msg_glo.h

@@ -49,5 +49,6 @@ void nav_msg_init_glo(nav_msg_glo_t *n);
 s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e);
 u32 extract_word_glo(const nav_msg_glo_t *n, u16 bit_index, u8 n_bits);
 s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val);
+s8 error_detection_glo(nav_msg_glo_t *n);
 
 #endif /* LIBSWIFTNAV_NAV_MSG_GLO_H */

src/nav_msg_glo.c

@@ -15,6 +15,7 @@
 #include <math.h>
 #include <libswiftnav/nav_msg_glo.h>
 #include <libswiftnav/time.h>
+#include <libswiftnav/logging.h>
 
 /* Word Ft (accuracy of measurements), refer to GLO ICD, Table 4.4 */
 const float f_t[] = { 1.0f, 2.0f, 2.5f, 4.0f, 5.0f, 7.0f, 10.0f, 12.0f, 14.0f,
@@ -23,6 +24,18 @@ const float f_t[] = { 1.0f, 2.0f, 2.5f, 4.0f, 5.0f, 7.0f, 10.0f, 12.0f, 14.0f,
 /* Word P1 (Time interval between adjacent values of tb, minutes), refer Table 4.3 */
 const u8 p1[] = { 0, 30, 45, 60 }; /* min */
 
+/* These bit masks (for data bits 9..85) correspond to table 4.13 of GLO ICD
+ * used in error correction algorithm */
+const u32 e_masks[7][3] = {
+    { 0xaaad5b00, 0x55555556, 0xaaaab  },
+    { 0x33366d00, 0x9999999b, 0xccccd  },
+    { 0xc3c78e00, 0xe1e1e1e3, 0x10f0f1 },
+    { 0xfc07f000, 0xfe01fe03, 0xff01   },
+    { 0xfff80000, 0xfffe0003, 0x1f0001 },
+    { 0,          0xfffffffc, 1        },
+    { 0,          0,          0x1ffffe },
+};
+
 /** Initialize the necessary parts of the nav message state structure.
  * \param n Pointer to GLO nav message structure to be initialized
  */
@@ -33,6 +46,97 @@ void nav_msg_init_glo(nav_msg_glo_t *n)
   n->state = SYNC_TM;
 }
 
+/* The algorithm based on
+ * https://graphics.stanford.edu/~seander/bithacks.html#ParityParallel
+ * \param in input data to calculate parity
+ * \param word if true, calculate parity for 32 bits otherwise for 8 bits
+ * \return parity bit */
+static bool parity(u32 in, bool word)
+{
+  u32 a = in;
+  if (word) {
+    a ^= a >> 16;
+    a ^= a >> 8;
+  } else
+    a &= 0x000000ff;
+  a ^= a >> 4;
+  a &= 0xf;
+  return (0x6996 >> a) & 1;
+}
+
+/** The function performs data verification and error correction
+ * in received GLO navigation string. Refer to GLO ICD, section 4.7
+ * \param n pointer to GLO nav message structure
+ * \return -1 -- received string is bad and should be dropped out,
+ *          0 -- received string is good
+ *          >0 -- number of bit in n->string_bits to be corrected (inverted)
+ *                range[9..85]*/
+s8 error_detection_glo(nav_msg_glo_t *n)
+{
+  u8 c = 0;
+  u32 data1, data2, data3;
+  bool p0, p1, p2, p3, beta, c_sum;
+  u8 bit_set = 0;
+  u8 k = 0;
+
+  /* calculate C1..7 */
+  for (u8 i = 0; i < 7; i++) {
+    /* extract corresponding check bit of Hamming code */
+    beta = extract_word_glo(n, i+1, 1);
+    /* extract data bits and apply mask */
+    data1 = extract_word_glo(n, 1, 32) & e_masks[i][0];
+    data2 = extract_word_glo(n, 33, 32) & e_masks[i][1];
+    data3 = extract_word_glo(n, 65, 32) & e_masks[i][2];
+    /* calculate parity for data[1..3] */
+    p1 = parity(data1, true);
+    p2 = parity(data2, true);
+    p3 = parity(data3, true);
+    bool p = beta ^ p1 ^ p2 ^ p3;
+    /* calculate common parity and set according C bit */
+    c |= p << i;
+    if (p) {
+      bit_set++; /* how many bits are set, used in error criteria */
+      k = i + 1; /* store number of most significant checksum not equal to 0,
+                    used in error criteria */
+    }
+  }
+
+  /* calculate C sum */
+  data1 = extract_word_glo(n, 1, 32) & 0xffffff00;
+  data2 = extract_word_glo(n, 33, 32);
+  data3 = extract_word_glo(n, 65, 32);
+  p1 = parity(data1, true);
+  p2 = parity(data2, true);
+  p3 = parity(data3, true);
+  p0 = parity(extract_word_glo(n, 1, 8), false);
+  c_sum = p0 ^ p1 ^ p2 ^ p3;
+
+  /* Now check C word to figure out is the string good, bad or
+   * correction is needed */
+  if ((!c_sum && !bit_set) || (1 == bit_set && c_sum)) /* case a) from ICD */
+
+    return 0; /* The string is good */
+
+  else if ((bit_set > 0 && !c_sum)
+           || (0 == bit_set && c_sum)) /* case c) from ICD */
+
+    return -1; /* multiple errors, bad string */
+
+  else if (bit_set > 1 && c_sum) { /* case b) from ICD */
+
+    u8 i_corr = (c & 0x7f) + 8 - k; /* define number of bit to be corrected */
+
+    if (i_corr > 85)
+      return -1; /* odd number of multiple errors, bad string */
+
+    return i_corr; /* return the bit to be corrected */
+
+  } else {/* should not be here */
+    log_error("GLO error correction: unexpected case");
+    return -1;
+  }
+}
+
 /** Extract a word of n_bits length (n_bits <= 32) at position bit_index into
  * the subframe. Refer to bit index to Table 4.6 and 4.11 in GLO ICD 5.1 (pg. 34)
  * \param n pointer to GLO nav message structure to be parsed

tests/check_glo_decoder.c

@@ -9,7 +9,7 @@
  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
  */
-#define DEBUG 1
+//#define DEBUG 1
 
 #include <stdio.h>
 #include <check.h>
@@ -168,7 +168,7 @@ START_TEST(test_nav_msg_update_glo)
     nav_msg_init_glo(&a);
     /* write test string to temporary buffer */
     memcpy(a.string_bits, strings_in[i], sizeof(n.string_bits));
-    /* transmit data bits, 85 bit */
+    /* transmit data bits, 85 bits */
     for (j = 85; j > 0; j--) {
       bool one_bit = extract_word_glo(&a, j, 1); /* get bit to be transmitted */
       manchester = (one_bit << 1 | one_bit) ^ 2; /* transform to line code */
@@ -188,6 +188,7 @@ START_TEST(test_nav_msg_update_glo)
 
       if (process_string_glo(&n, &e) == 1)
         e_out();
+
     }
     /* now pass time mark bit by bit to receiver (MSB first),
      * no line code needed */
@@ -199,13 +200,65 @@ START_TEST(test_nav_msg_update_glo)
 }
 END_TEST
 
+START_TEST(test_error_correction_glo)
+{
+  const struct {
+    u32 str_in[3]; /**< input string for test  */
+    s8 ret; /** result of the test */
+  } test_case[] = {
+    /* First, simply test one GLO nav message received from Novatel,
+     * we trust Novatel, so no errors must be */
+    { {0xc90cfb3e, 0x9743a301, 0x010749},  0}, /* case 0 */
+    { {0xdd39f5fc, 0x24542d0c, 0x021760},  0},
+    { {0x653bc7e9, 0x1e8ead92, 0x038006},  0},
+    { {0x60342dfc, 0x41000002, 0x0481c7},  0},
+    { {0x40000895, 0x00000003, 0x050d10},  0},
+    { {0x530a7ecf, 0x059c4415, 0x06b082},  0},
+    { {0xfd94beb6, 0x7a577e97, 0x070f46},  0},
+    { {0xba02de6f, 0x988e6814, 0x08b101},  0},
+    { {0x12064831, 0x87767698, 0x09e1a6},  0},
+    { {0xaf870be5, 0x54ef2617, 0x0ab286},  0},
+    { {0x0f06ba41, 0x9a3f2698, 0x0b8f7c},  0},
+    { {0x2f012204, 0xf0c3c81a, 0x0cb309},  0},
+    { {0x1c858601, 0x10c47e98, 0x0da065},  0},
+    { {0x5205980b, 0xf49abc1a, 0x0eb40e},  0},
+    { {0x15454437, 0x2504e698, 0x0f8c09},  0},
+    /* Second, take 1st string from other GLO nav message and introduce an error
+     * in data bits */
+    { {0xc90cfb81, 0x9743a301, 0x010748},  0}, /* case 15, no errors  */
+    { {0xc90cfb81, 0x9743a301, 0x110748}, 85},
+    { {0xc90cfb81, 0x1743a301, 0x010748}, 64},
+    { {0x490cfb81, 0x9743a301, 0x010748}, 32},
+    { {0xc90cfb81, 0x9743a300, 0x010748}, 33},
+    { {0xc90cfb81, 0x9743a301, 0x010749}, 65},
+    { {0xc90cfb81, 0x9743a301, 0x000748}, 81},
+    { {0xc90c3b81, 0x9743a301, 0x010748}, -1},
+    { {0xc90cfb81, 0x974fa301, 0x010748}, -1},
+    { {0xc90cfb81, 0x9743a301, 0x01074b}, -1},
+    { {0xc90cfb81, 0x9743a301, 0x010744}, -1},
+    { {0xc90cfb81, 0x9aaaa301, 0x010748}, -1},
+    { {0xc90cfb81, 0x9743a301, 0x010748},  0}, /* no errors here */
+  };
+
+  nav_msg_glo_t n;
+
+  for(u8 i = 0; i < sizeof(test_case) / sizeof(test_case[0]); i++) {
+    memcpy(n.string_bits, test_case[i].str_in, sizeof(n.string_bits));
+    s8 ret = error_detection_glo(&n);
+    fail_unless(test_case[i].ret == ret, "Case %u: ret = %d, expected %d",
+                i, ret, test_case[i].ret);
+  }
+}
+END_TEST
+
 Suite* glo_decoder_test_suite(void)
 {
   Suite *s = suite_create("GLO decoder");
   TCase *tc_core = tcase_create("Core");
   tcase_add_test(tc_core, test_extract_glo_word);
   tcase_add_test(tc_core, test_process_string_glo);
   tcase_add_test(tc_core, test_nav_msg_update_glo);
+  tcase_add_test(tc_core, test_error_correction_glo);
   suite_add_tcase(s, tc_core);
 
   return s;