Merge pull request #337 from dt-exafore/i123-glo-decoder-error-corrections

I123 glo decoder error detector

Author: Roman Gezikov

include/libswiftnav/nav_msg_glo.h

@@ -23,6 +23,9 @@
 /** Length of GLO time mark */
 #define GLO_TM_LEN 30
 
+/** Length of GLO navigation string */
+#define GLO_STR_LEN 85
+
 /* States of receiver GLO bitstream */
 typedef enum {
   INVALID = -1,
@@ -43,11 +46,14 @@ typedef struct {
   glo_receive_machine state; /**< current state of receiver */
   u8 meander_bits_cnt:2; /**< counter for line code bits, MAX is 2 */
   u8 manchester:2; /**< 2 bits line code received */
+  u8 decode_done:1; /**< decode done flag */
+  u8 inverted:1; /**< incoming data is inverted */
 } nav_msg_glo_t;
 
 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(const nav_msg_glo_t *n);
+double nav_msg_get_tow_glo(const nav_msg_glo_t *n);
 
 #endif /* LIBSWIFTNAV_NAV_MSG_GLO_H */

src/nav_msg_glo.c

@@ -15,6 +15,8 @@
 #include <math.h>
 #include <libswiftnav/nav_msg_glo.h>
 #include <libswiftnav/time.h>
+#include <libswiftnav/logging.h>
+#include <libswiftnav/bits.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 +25,20 @@ 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 },
+};
+
+static u32 extract_word_glo(const nav_msg_glo_t *n, u16 bit_index, u8 n_bits);
+
 /** Initialize the necessary parts of the nav message state structure.
  * \param n Pointer to GLO nav message structure to be initialized
  */
@@ -33,17 +49,97 @@ void nav_msg_init_glo(nav_msg_glo_t *n)
   n->state = SYNC_TM;
 }
 
+/** 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(const 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);
+    p2 = parity(data2);
+    p3 = parity(data3);
+    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);
+  p2 = parity(data2);
+  p3 = parity(data3);
+  p0 = parity(extract_word_glo(n, 1, 8));
+  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 */
+
+  if ((bit_set > 0 && !c_sum)
+           || (0 == bit_set && c_sum)) /* case c) from ICD */
+
+    return -1; /* multiple errors, bad string */
+
+  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 > GLO_STR_LEN)
+      return -1; /* odd number of multiple errors, bad string */
+
+    return i_corr; /* return the bit to be corrected */
+  }
+
+  /* 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
  * \param bit_index number of bit the extract process start with. Range [1..85]
  * \param n_bits how many bits should be extracted [1..32]
  * \return word extracted from navigation string
  */
-u32 extract_word_glo(const nav_msg_glo_t *n, u16 bit_index, u8 n_bits)
+static u32 extract_word_glo(const nav_msg_glo_t *n, u16 bit_index, u8 n_bits)
 {
-  assert(n_bits <= 32 && n_bits > 0);
-  assert(bit_index <= 85 && bit_index > 0);
+  if (bit_index > GLO_STR_LEN || !bit_index) {
+    log_error("Incorrect bit index %d\n", bit_index);
+    return 0;
+  }
+
+  if (n_bits > 32 || !n_bits ) {
+    log_error("Incorrect number of bits to be extracted %d\n", n_bits);
+    return 0;
+  }
 
   /* Extract a word of n_bits length (n_bits <= 32) at position bit_index into
    * the GLO string.*/
@@ -83,12 +179,17 @@ s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val)
     n->string_bits[0] <<= 1; /* use one word of buffer for that purpose */
     n->string_bits[0] |= bit_val;
     /* collected bits match time mark? if not stay at this state */
-    if (extract_word_glo(n, 1, GLO_TM_LEN) == GLO_TM) {
+    u32 tm = extract_word_glo(n, 1, GLO_TM_LEN);
+    if (tm == GLO_TM || tm == (u32)(~GLO_TM & 0x3fffffff)) {
       /* time mark found, next time start collecting data bits */
       n->meander_bits_cnt = 0;
       n->manchester = 0;
       n->state = GET_DATA_BIT;
       n->string_bits[0] = 0;
+      if (tm == GLO_TM)
+        n->inverted = 0;
+      else if (tm == (u32)(~GLO_TM & 0x3fffffff))
+        n->inverted = 1;
     }
     break;
   case GET_DATA_BIT: /* collect data bits of string */
@@ -106,14 +207,17 @@ s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val)
         n->string_bits[i] = tmp;
       }
       n->string_bits[0] <<= 1;
+
+      /* set type of meander depending on inversion */
+      u8 meander = n->inverted ? 1 : 2;
       /* store bit after meander removal to buffer */
-      n->string_bits[0] |= (n->manchester ^ 2) & 1;
+      n->string_bits[0] |= (n->manchester ^ meander) & 1;
       n->current_head_bit_index++;
       n->meander_bits_cnt = 0;
       n->manchester = 0;
       /* did we received all bits of a string?
        * if yes, notify user and start searching time mark again*/
-      if (n->current_head_bit_index == 85) {
+      if (n->current_head_bit_index == GLO_STR_LEN) {
         n->current_head_bit_index = 0;
         n->state = SYNC_TM;
         ret = 1;
@@ -138,7 +242,7 @@ s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val)
 s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
 {
   /* Extract and check dummy bit from GLO string, bit 85 in GLO string */
-  if (extract_word_glo(n, 85, 1) != 0) {
+  if (extract_word_glo(n, GLO_STR_LEN, 1) != 0) {
     log_error("GLO dummy bit is not 0.");
     return -1;
   }
@@ -148,6 +252,7 @@ s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
   u8 sign;
   /* is the string we need? */
   if (n->next_string_id == m) {
+    n->decode_done = 0;
     switch (m) {
     case 1: /* string 1 */
       /* extract x */
@@ -238,24 +343,38 @@ s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
       /* extract N4 */
       n->n4 = (u8) extract_word_glo(n, 32, 5);
 
-      n->next_string_id = 0; /* all string parsed */
+      n->next_string_id = 1; /* start decode from 1st string next time */
+      n->decode_done = 1; /* all string parsed */
       break;
     default:
       break;
     }
   }
   /* all needed strings decoded?
    * fill ephemeris structure if we was not able to do it before*/
-  if (n->next_string_id == 0) {
+  if (n->decode_done) {
     e->sid.code = CODE_GLO_L1CA;
     /* convert GLO TOE to GPS TOE */
     e->toe = glo_time2gps_time(n->nt, n->n4, n->hrs, n->min, n->sec);
     e->healthy ^= 1; /* invert healthy bit */
     e->valid = e->healthy; //NOTE: probably Valid needs to be defined by other way
-    n->next_string_id = 1; /* start from string 1 */
     return 1;
   }
 
-
   return 0;
 }
+
+/** This function just a wrapper for glo_time2gps_time
+ * \param n pointer to GLO nav message
+ * \return time of GLO referenced to the end of the 5th GLO nav string presented as
+ *         GPS time, -1 if ephemeris cannot be decoded */
+double nav_msg_get_tow_glo(const nav_msg_glo_t *n)
+{
+  /* + 10 sec for the end of 5th string */
+  if (!n->decode_done)
+    return -1;
+  else {
+    gps_time_t t = glo_time2gps_time(n->nt, n->n4, n->hrs, n->min, n->sec+10);
+    return t.tow;
+  }
+}