Merge pull request #294 from dt-exafore/i38-l2c-capability-monitor

Add decoding of L2C capability mask

Author: Roman Gezikov

include/libswiftnav/ephemeris.h

@@ -107,7 +107,7 @@ u8 ephemeris_params_valid(const u8 valid, const u32 fit_interval,
                       const gps_time_t* toe, const gps_time_t *t);
 u8 satellite_healthy(const ephemeris_t *e);
 
-void decode_ephemeris(u32 frame_words[3][8], ephemeris_t *e);
+void decode_ephemeris(u32 frame_words[4][8], ephemeris_t *e);
 bool ephemeris_equal(const ephemeris_t *a, const ephemeris_t *b);
 
 #endif /* LIBSWIFTNAV_EPHEMERIS_H */

include/libswiftnav/l2c_capability.h

@@ -0,0 +1,21 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Dmitry Tatarinov <dmitry.tatarinov@exafore.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+#ifndef LIBSWIFTNAV_L2C_CAPABILITY_H
+#define LIBSWIFTNAV_L2C_CAPABILITY_H
+
+#include <libswiftnav/common.h>
+#include <libswiftnav/nav_msg.h>
+
+void decode_l2c_capability(const u32 *subframe4_words, u32 *l2c_cpbl);
+
+#endif /* LIBSWIFTNAV_L2C_CAPABILITY_H */

include/libswiftnav/nav_msg.h

@@ -35,16 +35,26 @@ typedef struct {
    */
   s16 subframe_start_index;
 
-  u32 frame_words[3][8];
+  u32 frame_words[4][8];
   u8 next_subframe_id;
   s8 bit_polarity;
 
   u8 alert;
 } nav_msg_t;
 
+typedef struct {
+  ephemeris_t ephemeris;
+  bool ephemeris_upd_flag;
+
+  u32 gps_l2c_sv_capability;
+  bool gps_l2c_sv_capability_upd_flag;
+} gps_l1ca_decoded_data_t;
+
+
 void nav_msg_init(nav_msg_t *n);
 s32 nav_msg_update(nav_msg_t *n, bool bit_val);
 bool subframe_ready(nav_msg_t *n);
-s8 process_subframe(nav_msg_t *n, ephemeris_t *e);
+s8 process_subframe(nav_msg_t *n, gnss_signal_t sid,
+                    gps_l1ca_decoded_data_t *data);
 
 #endif /* LIBSWIFTNAV_NAV_MSG_H */

python/swiftnav/nav_msg.pxd

@@ -30,10 +30,17 @@ cdef extern from "libswiftnav/nav_msg.h":
     u8 next_subframe_id
     s8 bit_polarity
 
+  ctypedef struct gps_l1ca_decoded_data_t:
+    ephemeris_t ephemeris
+    bool ephemeris_upd_flag
+    u32 gps_l2c_sv_capability
+    bool gps_l2c_sv_capability_upd_flag
+
   void nav_msg_init(nav_msg_t *n)
   s32 nav_msg_update(nav_msg_t *n, bool bit_val)
   bool subframe_ready(nav_msg_t *n)
-  s8 process_subframe(nav_msg_t *n, ephemeris_t *e)
+  s8 process_subframe(nav_msg_t *n, gnss_signal_t sid,
+                      gps_l1ca_decoded_data_t *data)
 
 cdef class NavMsg:
   cdef nav_msg_t _thisptr

python/swiftnav/nav_msg.pyx

@@ -40,9 +40,9 @@ cdef class NavMsg:
   def subframe_ready(self):
     return subframe_ready(&self._thisptr)
 
-  def process_subframe(self, e):
-    cdef ephemeris_t tmp = e._thisptr
-    return process_subframe(&self._thisptr, &tmp)
+  def process_subframe(self, sid, d):
+    cdef gps_l1ca_decoded_data_t tmp_d = d._thisptr
+    return process_subframe(&self._thisptr, sid, &tmp_d)
 
   def __richcmp__(self, other, op):
     """

src/CMakeLists.txt

@@ -45,6 +45,7 @@ set(libswiftnav_SRCS
   signal.c
   ionosphere.c
   bit_sync.c
+  l2c_capability.c
   cnav_msg.c
   nav_msg_glo.c
   ${plover_SRCS}

src/ephemeris.c

@@ -596,7 +596,7 @@ u32 decode_fit_interval(u8 fit_interval_flag, u16 iodc) {
  *                    1, 2 and 3. Word is in the 30 LSBs of the u32.
  * \param e Pointer to an ephemeris struct to fill in.
  */
-void decode_ephemeris(u32 frame_words[3][8], ephemeris_t *e)
+void decode_ephemeris(u32 frame_words[4][8], ephemeris_t *e)
 {
   assert(frame_words != NULL);
   assert(e != NULL);

src/l2c_capability.c

@@ -0,0 +1,67 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Dmitry Tatarinov <dmitry.tatarinov@exafore.com>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+#include <stdio.h>
+#include <assert.h>
+
+#include <libswiftnav/common.h>
+#include <libswiftnav/nav_msg.h>
+#include <libswiftnav/l2c_capability.h>
+
+/** \defgroup l2c_capability L2C Capability
+ * Functions used in l2c capability
+ * \{ */
+
+/** Decode L2C capability data from Nav message.
+ * \param subframe4_words Array containing words 3 through 10
+ *                        of subframe 4.
+ *                        SV configuration data is in words 3 through 8
+ * \param l2c_cpbl output Pointer to 32-bit L2C capability container
+ *
+ */
+void decode_l2c_capability(const u32 *subframe4_words, u32 *l2c_cpbl)
+{
+  assert(subframe4_words != NULL);
+  assert(l2c_cpbl != NULL);
+
+  *l2c_cpbl = 0;
+
+  struct sv_conf_location {
+    u8 n_sv; /* for how many SVs config data in the Word */
+    u8 end_bit_n; /* position of MSB for the 1st SV in the Word */
+  };
+
+  const struct sv_conf_location sv_conf_loc[6] = {
+    {4,12},{6,4},{6,4},{6,4},{6,4},{4,4},
+  };
+
+  u8 sv_id = 0;
+
+  /* go through words 3-8 */
+  for (u8 i = 3; i <= 8; i++ ) {
+
+    /* go through all sv inside the word */
+    for (u8 j = 0; j < sv_conf_loc[i-3].n_sv; j++ ) {
+      /* get SV config bits take into account only 3 LSB */
+      u8 sv_conf =
+      subframe4_words[i-3] >> (30 - (sv_conf_loc[i-3].end_bit_n + j*4)) & 7;
+
+      /* set or clear appropriate capability bit,
+       * refer pg. 115-116 of IS-200H for the criteria */
+      if (2 == sv_conf || 3 == sv_conf || 4 == sv_conf)
+        *l2c_cpbl |= (1 << sv_id);
+
+      sv_id++;
+    }
+  }
+}
+
+/** \} */

src/nav_msg.c

@@ -20,6 +20,7 @@
 #include <libswiftnav/constants.h>
 #include <libswiftnav/bits.h>
 #include <libswiftnav/nav_msg.h>
+#include <libswiftnav/l2c_capability.h>
 
 void nav_msg_init(nav_msg_t *n)
 {
@@ -222,16 +223,13 @@ static u8 nav_parity(u32 *word)
 bool subframe_ready(nav_msg_t *n) {
   return (n->subframe_start_index != 0);
 }
-
-s8 process_subframe(nav_msg_t *n, ephemeris_t *e) {
+s8 process_subframe(nav_msg_t *n, gnss_signal_t sid,
+                    gps_l1ca_decoded_data_t *data)
+{
   // Check parity and parse out the ephemeris from the most recently received subframe
 
-  if (!e) {
-    log_error("process_subframe: CALLED WITH e = NULL!");
-    n->subframe_start_index = 0;  // Mark the subframe as processed
-    n->next_subframe_id = 1;      // Make sure we start again next time
-    return -1;
-  }
+  assert(data != NULL);
+  memset(data, 0, sizeof(gps_l1ca_decoded_data_t));
 
   // First things first - check the parity, and invert bits if necessary.
   // process the data, skipping the first word, TLM, and starting with HOW
@@ -242,40 +240,40 @@ s8 process_subframe(nav_msg_t *n, ephemeris_t *e) {
     BIT_POLARITY_INVERTED;
   if ((prev_bit_polarity != BIT_POLARITY_UNKNOWN)
       && (prev_bit_polarity != n->bit_polarity)) {
-    log_warn_sid(e->sid, "Nav phase flip - half cycle slip detected, "
+    log_warn_sid(sid, "Nav phase flip - half cycle slip detected, "
                  "but not corrected");
     /* TODO: declare phase ambiguity to IAR */
   }
 
   /* Complain if buffer overrun */
   if (n->overrun) {
-    log_error_sid(e->sid, "nav_msg subframe buffer overrun!");
+    log_error_sid(sid, "nav_msg subframe buffer overrun!");
     n->overrun = false;
   }
 
   /* Extract word 2, and the last two parity bits of word 1 */
   u32 sf_word2 = extract_word(n, 28, 32, 0);
   if (nav_parity(&sf_word2)) {
-    log_info_sid(e->sid, "subframe parity mismatch (word 2)");
+    log_info_sid(sid, "subframe parity mismatch (word 2)");
     n->subframe_start_index = 0;  // Mark the subframe as processed
     n->next_subframe_id = 1;      // Make sure we start again next time
     return -2;
   }
 
   n->alert = sf_word2 >> 12 & 0x01; // Alert flag, bit 18
   if (n->alert) {
-    log_warn_sid(e->sid, "alert flag set! Ignoring satellite.");
+    log_warn_sid(sid, "alert flag set! Ignoring satellite.");
   }
 
   u8 sf_id = sf_word2 >> 8 & 0x07;    // Which of 5 possible subframes is it? bits 20-22
 
-  if (sf_id <= 3 && sf_id == n->next_subframe_id) {  // Is it the one that we want next?
+  if (sf_id <= 4 && sf_id == n->next_subframe_id) {  // Is it the one that we want next?
 
     for (int w = 0; w < 8; w++) {   // For words 3..10
       n->frame_words[sf_id-1][w] = extract_word(n, 30*(w+2) - 2, 32, 0);    // Get the bits
       // MSBs are D29* and D30*.  LSBs are D1...D30
       if (nav_parity(&n->frame_words[sf_id-1][w])) {  // Check parity and invert bits if D30*
-        log_info_sid(e->sid, "subframe parity mismatch (word %d)", w+3);
+        log_info_sid(sid, "subframe parity mismatch (word %d)", w+3);
         n->next_subframe_id = 1;      // Make sure we start again next time
         n->subframe_start_index = 0;  // Mark the subframe as processed
         return -3;
@@ -285,14 +283,30 @@ s8 process_subframe(nav_msg_t *n, ephemeris_t *e) {
     n->next_subframe_id++;
 
     if (sf_id == 3) {
-      // Got all of subframes 1 to 3
-      n->next_subframe_id = 1;      // Make sure we start again next time
 
       // Now let's actually decode the ephemeris...
-      decode_ephemeris(n->frame_words, e);
+      data->ephemeris.sid = sid;
+      decode_ephemeris(n->frame_words, &data->ephemeris);
+      data->ephemeris_upd_flag = true;
 
       return 1;
+    }
+
+    if (4 == sf_id) { /* parse Subframe 4 */
+       /*  get words 3-8 from 25th page (SV config
+       *  bits) */
+
+      /* check Word 3 bits 2..7 (63..69) for Page ID, see IS-200H, pg. 84
+       * Page 25 has ID 63, see IS-200H, pg. 109-110 */
+      if ((n->frame_words[3][3-3] >> (30-8) & 0x3f) == 63) {
+        decode_l2c_capability(n->frame_words[3], &(data->gps_l2c_sv_capability));
+        data->gps_l2c_sv_capability_upd_flag = true;
+      }
+
+      /* Got all of subframes 1 to 4 */
+      n->next_subframe_id = 1; /* Make sure we start again next time */
 
+      return 1;
     }
   } else {  // didn't get the subframe that we want next
       n->next_subframe_id = 1;      // Make sure we start again next time

tests/CMakeLists.txt

@@ -47,6 +47,7 @@ else (CMAKE_CROSSCOMPILING)
       check_ionosphere.c
       check_signal.c
       check_track.c
+      check_decode_l2c_capability.c
       check_cnav.c
       check_glo_decoder.c
     )