Add GLO bitstream receiver

Author: Dmitry Tatarinov

include/libswiftnav/nav_msg_glo.h

@@ -18,13 +18,29 @@
 
 #define NAV_MSG_GLO_STRING_BITS_LEN 3 /* Buffer 96 nav bits. */
 
+/** Time mark in GLO nav string, GLO ICD, pg. 16 */
+#define GLO_TM (0x3E375096)
+#define GLO_TM_LEN 30
+
+typedef enum {
+  INVALID = -1,
+  SYNC_TM,
+  GET_DATA_BIT
+} glo_decode_machine;
+
 typedef struct {
   u32 string_bits[NAV_MSG_GLO_STRING_BITS_LEN];
+  u32 tm_buff;
+  u16 current_head_bit_index;
   u8 next_string_id;
+  u8 meander_bits_cnt;
+  u8 manchester;
+  glo_decode_machine state;
 } 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); //TODO: remove after tests
+s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val);
 
 #endif /* LIBSWIFTNAV_NAV_MSG_GLO_H */

src/nav_msg_glo.c

@@ -51,6 +51,7 @@ void nav_msg_init_glo(nav_msg_glo_t *n)
   /* Initialize the necessary parts of the nav message state structure. */
   memset(n, 0, sizeof(nav_msg_glo_t));
   n->next_string_id = 1; /* start parsing from string 1 */
+  n->state = SYNC_TM;
   done_flag = false;
 }
 
@@ -84,7 +85,70 @@ void nav_msg_init_glo(nav_msg_glo_t *n)
   return word;
 }
 
-/** The function deccodes a GLO navigation string
+/** Navigation message GLO decoding update.
+ * Called once per nav bit interval (10 ms).
+ * Performs the necessary steps to store the nav bits in buffer.
+ *
+ * \param n GLO Nav message decode state struct
+ * \param bit_val State of the nav bit to process, 0 or 1
+ *
+ * \return  1 if Glo nav string ready for decoding,
+ *         -1 otherwise.
+ */
+s8 nav_msg_update_glo(nav_msg_glo_t *n, bool bit_val)
+{
+  s8 ret = -1;
+
+  switch (n->state) {
+    case SYNC_TM: /* try to find time mark */
+      /* put incoming bit at the tail of the buffer */
+      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) {
+        /* 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;
+      }
+      break;
+    case GET_DATA_BIT: /* collect data bits of string */
+      n->meander_bits_cnt++;
+      n->manchester <<= 1;
+      n->manchester |= bit_val; /* store incoming bit */
+      /* did we take 2 bits of line code?
+       * if yes, remove meander and store bit in buffer,
+       * if no, stay at the state */
+      if (n->meander_bits_cnt == 2) {
+        /* shift whole buffer by 1 bit left */
+        for (u8 i = NAV_MSG_GLO_STRING_BITS_LEN - 1; i > 0; i--) {
+          u32 tmp = (n->string_bits[i] << 1) | ((n->string_bits[i-1] & (1<<31)) >> 31);
+          n->string_bits[i] = tmp;
+        }
+        n->string_bits[0] <<= 1;
+        /* store bit after meander removal to buffer */
+        n->string_bits[0] |= (n->manchester ^ 2) & 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) {
+          n->current_head_bit_index = 0;
+          n->state = SYNC_TM;
+          ret = 1;
+        }
+      }
+      break;
+    default:
+      nav_msg_init_glo(n); //TODO: probably not needed to initialize next_string_id
+      break;
+  }
+  return ret;
+}
+
+/** The function decodes a GLO navigation string
  * \param n Pointer to nav_msg_glo_t structure which contains GLO string
  * \param e Pointer to Ephemeris to store
  * \return 0 - decode not completed,
@@ -144,7 +208,7 @@ s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
           sign = extract_word_glo(n, 36 + 4, 1);
           g_e.glo.acc[1] = sign ? -1.0 * ret * pow(2, -30) * 1000.0 :
                                          ret * pow(2, -30) * 1000.0;
-          /* extract MSB of B */
+          /* extract MSB of B (if the bit is clear the SV is OK ) */
           g_e.healthy = extract_word_glo(n, 80, 1);
           /* extract P1 */
           g_e.fit_interval = p1[extract_word_glo(n, 77, 2)];
@@ -172,7 +236,7 @@ s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
           sign = extract_word_glo(n, 69 + 10, 1);
           g_e.glo.gamma = sign ? -1.0 * ret * pow(2, -40) :
                                         ret * pow(2, -40);
-          /* extract l and add B */
+          /* extract l, if the it is clear the SV is OK, so OR it with B */
           g_e.healthy |= extract_word_glo(n, 65, 1);
 
           n->next_string_id = 4;
@@ -208,6 +272,7 @@ s8 process_string_glo(nav_msg_glo_t *n, ephemeris_t *e)
       g_e.sid.code = CODE_GLO_L1CA;
       /* convert GLO TOE to GPS TOE */
       g_e.toe = glo_time2gps_time(nt, n4, hrs, min, sec);
+      g_e.healthy ^= 1; /* invert healthy bit */
       g_e.valid = g_e.healthy; //NOTE: probably Valid needs to be defined by other way
       memcpy(e, &g_e, sizeof(g_e));
       done_flag = false;

tests/check_glo_decoder.c

@@ -14,6 +14,7 @@
 #include <stdio.h>
 #include <check.h>
 #include <math.h>
+#include <string.h>
 #include <libswiftnav/nav_msg_glo.h>
 #include <libswiftnav/logging.h>
 
@@ -29,7 +30,8 @@ ephemeris_t e;
  * 0aad8090a54019cb035d3f,0,0b3e2240201e97fc34fc39,0,0cae940cdc3c1e2786da9b,0,
  * 0d68bf54a4c697f115320b,0,0eaf8449b38c1e228932d8,0,0f815b653eee981314802a,0*f3837a1c
 */
-u32 strings_in[5][3] = {
+u32 strings_in[6][3] = {
+  {1,1,1},                            /* dummy words used in test_nav_msg_update_glo only */
   {0xc3a850b5, 0x96999b05, 0x010743}, /* 01074396999b05c3a850b5 */
   {0xd9c15f66, 0xa5256204, 0x021760}, /* 021760a5256204d9c15f66 */
   {0x6d0e3123, 0x9d60899a, 0x038026}, /* 0380269d60899a6d0e3123 */
@@ -60,6 +62,36 @@ double AZ = -2.79396772384643555e-06;
 double GAMMA = 1.81898940354585648e-12;
 double TAU = -9.71024855971336365e-05;
 
+void e_out(void)
+{
+  log_debug("GLO Ephemeris:\n");
+  log_debug("\tSID: %u (code %u)\n", e.sid.sat, e.sid.code);
+  log_debug("\tGPS time: TOE %f, WN %d\n", e.toe.tow, e.toe.wn);
+  log_debug("\tURA: %f\n", e.ura);
+  log_debug("\tFit interval: %u\n", e.fit_interval);
+  log_debug("\tValid: %u\n", e.valid);
+  log_debug("\tHealthy: %u\n", e.healthy);
+  log_debug("\tgamma: %25.18f\n", e.glo.gamma);
+  log_debug("\ttau: %25.18f\n", e.glo.tau);
+  log_debug("\tX, Y, Z: %25.18f, %25.18f, %25.18f\n",
+             e.glo.pos[0], e.glo.pos[1], e.glo.pos[2]);
+  log_debug("\tVX, VY, VZ: %25.18f, %25.18f, %25.18f\n",
+             e.glo.vel[0], e.glo.vel[1], e.glo.vel[2]);
+  log_debug("\tAX, AY, AZ: %25.18f, %25.18f, %25.18f\n",
+             e.glo.acc[0], e.glo.acc[1], e.glo.acc[2]);
+  fail_unless(e.glo.pos[0]-X == 0, "dX %25.18f, expected %25.18f", e.glo.pos[0], X);
+  fail_unless(e.glo.pos[1]-Y == 0, "dY %25.18f, expected %25.18f", e.glo.pos[1], Y);
+  fail_unless(e.glo.pos[2]-Z == 0, "dZ %25.18f, expected %25.18f", e.glo.pos[2], Z);
+  fail_unless(e.glo.vel[0]-VX == 0, "dVX %25.18f, expected %25.18f", e.glo.vel[0], VX);
+  fail_unless(e.glo.vel[1]-VY == 0, "dVY %25.18f, expected %25.18f", e.glo.vel[1], VY);
+  fail_unless(e.glo.vel[2]-VZ == 0, "dVZ %25.18f, expected %25.18f", e.glo.vel[2], VZ);
+  fail_unless(e.glo.acc[0]-AX == 0, "dAX %25.18f, expected %25.18f", e.glo.acc[0], AX);
+  fail_unless(e.glo.acc[1]-AY == 0, "dAY %25.18f, expected %25.18f", e.glo.acc[1], AY);
+  fail_unless(e.glo.acc[2]-AZ == 0, "dAZ %25.18f, expected %25.18f", e.glo.acc[2], AZ);
+  fail_unless(e.glo.tau-TAU == 0, "dTAU %25.18f, expected %25.18f", e.glo.tau, TAU);
+  fail_unless(e.glo.gamma-GAMMA == 0, "dGAMMA %25.18f, expected %25.18f", e.glo.gamma, GAMMA);
+}
+
 START_TEST(test_extract_glo_word)
 {
   u32 ret = 0;
@@ -101,44 +133,66 @@ END_TEST
 START_TEST(test_process_string_glo)
 {
   nav_msg_init_glo(&n);
-  for(u8 i = 0; i < sizeof(strings_in)/sizeof(strings_in[0]); i++) {
+  memset(&e,0,sizeof(e));
+  for(u8 i = 1; i < sizeof(strings_in)/sizeof(strings_in[1]); i++) {
     memcpy(n.string_bits, strings_in[i], sizeof(n.string_bits));
     process_string_glo(&n, &e);
   }
-  log_debug("GLO Ephemeris:\n");
-  log_debug("\tSID: %u (code %u)\n", e.sid.sat, e.sid.code);
-  log_debug("\tGPS time: TOE %f, WN %d\n", e.toe.tow, e.toe.wn);
-  log_debug("\tURA: %f\n", e.ura);
-  log_debug("\tFit interval: %u\n", e.fit_interval);
-  log_debug("\tValid: %u\n", e.valid);
-  log_debug("\tHealthy: %u\n", e.healthy);
-  log_debug("\tgamma: %25.18f\n", e.glo.gamma);
-  log_debug("\ttau: %25.18f\n", e.glo.tau);
-  log_debug("\tX, Y, Z: %25.18f, %25.18f, %25.18f\n",
-             e.glo.pos[0], e.glo.pos[1], e.glo.pos[2]);
-  log_debug("\tVX, VY, VZ: %25.18f, %25.18f, %25.18f\n",
-             e.glo.vel[0], e.glo.vel[1], e.glo.vel[2]);
-  log_debug("\tAX, AY, AZ: %25.18f, %25.18f, %25.18f\n",
-             e.glo.acc[0], e.glo.acc[1], e.glo.acc[2]);
-  fail_unless(e.glo.pos[0]-X == 0, "dX %25.18f, expected %25.18f", e.glo.pos[0], X);
-  fail_unless(e.glo.pos[1]-Y == 0, "dY %25.18f, expected %25.18f", e.glo.pos[1], Y);
-  fail_unless(e.glo.pos[2]-Z == 0, "dZ %25.18f, expected %25.18f", e.glo.pos[2], Z);
-  fail_unless(e.glo.vel[0]-VX == 0, "dVX %25.18f, expected %25.18f", e.glo.vel[0], VX);
-  fail_unless(e.glo.vel[1]-VY == 0, "dVY %25.18f, expected %25.18f", e.glo.vel[1], VY);
-  fail_unless(e.glo.vel[2]-VZ == 0, "dVZ %25.18f, expected %25.18f", e.glo.vel[2], VZ);
-  fail_unless(e.glo.acc[0]-AX == 0, "dAX %25.18f, expected %25.18f", e.glo.acc[0], AX);
-  fail_unless(e.glo.acc[1]-AY == 0, "dAY %25.18f, expected %25.18f", e.glo.acc[1], AY);
-  fail_unless(e.glo.acc[2]-AZ == 0, "dAZ %25.18f, expected %25.18f", e.glo.acc[2], AZ);
-  fail_unless(e.glo.tau-TAU == 0, "dTAU %25.18f, expected %25.18f", e.glo.tau, TAU);
-  fail_unless(e.glo.gamma-GAMMA == 0, "dGAMMA %25.18f, expected %25.18f", e.glo.gamma, GAMMA);
+  e_out();
+}
+END_TEST
+
+START_TEST(test_nav_msg_update_glo)
+{
+  /* the unit test encodes strings_in to generate glo bitstream, calls
+   * nav_msg_update_glo to receive and finally decodes received string */
+  nav_msg_init_glo(&n);
+  memset(&e,0,sizeof(e));
+  /* get string one by one */
+  for(u8 i = 0; i < sizeof(strings_in)/sizeof(strings_in[0]); i++) {
+    u8 manchester = 0;
+    nav_msg_glo_t a;
+    s8 ret;
+    u8 j;
+    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 */
+    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 */
+      /* now pass it to receiver MSB first, receiver must return -1 */
+      ret = nav_msg_update_glo(&n, (manchester >> 1) & 1);
+      fail_unless(ret == -1, "ret = %d, expected -1", ret);
+      /* now LSB, receiver must return -1 */
+      ret = nav_msg_update_glo(&n, manchester & 1);
+    }
+    /* try to decode the string */
+    if (ret == 1) {
+      fail_unless(memcmp(a.string_bits, n.string_bits, sizeof(a.string_bits)) == 0,
+                  "Received string %x%x%x not equal to trasmitted one %x%x%x",
+                  n.string_bits[2],n.string_bits[1],n.string_bits[0],
+                  a.string_bits[2],a.string_bits[1],a.string_bits[0]);
+
+      if(process_string_glo(&n, &e) == 1)
+        e_out();
+    }
+    /* now pass time mark bit by bit to receiver (MSB first), no line code needed */
+    for (u8 j = 30; j > 0; j--) {
+      ret = nav_msg_update_glo(&n, (GLO_TM >> (j-1)) & 1);
+      fail_unless(ret == -1, "ret = %d, expected -1", 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);
   suite_add_tcase(s, tc_core);
 
   return s;