Merge pull request #350 from rge-exafore/libswiftnav-peregrine-l2c

Change libswiftnav to support L2CM tracking in Peregrine

Author: pmiettinen

include/libswiftnav/correlate.h

@@ -1,6 +1,7 @@
 /*
- * Copyright (C) 2013 Swift Navigation Inc.
+ * Copyright (C) 2013,2016 Swift Navigation Inc.
  * Contact: Fergus Noble <[email protected]>
+ * Contact: Adel Mamin <[email protected]>
  *
  * 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.
@@ -15,12 +16,22 @@
 
 #include <libswiftnav/common.h>
 
-void track_correlate(s8* samples, s8* code,
-                     double* init_code_phase, double code_step,
-                     double* init_carr_phase, double carr_step,
-                     double* I_E, double* Q_E,
-                     double* I_P, double* Q_P,
-                     double* I_L, double* Q_L,
-                     u32* num_samples);
+void l1_ca_track_correlate(const s8* samples, size_t samples_len,
+                           const s8* code,
+                           u32 chips_to_correlate,
+                           double* init_code_phase, double code_step,
+                           double* init_carr_phase, double carr_step,
+                           double* I_E, double* Q_E,
+                           double* I_P, double* Q_P,
+                           double* I_L, double* Q_L, u32* num_samples);
+
+void l2c_cm_track_correlate(const s8* samples, size_t samples_len,
+                            const s8* code,
+                            u32 chips_to_correlate,
+                            double* init_code_phase, double code_step,
+                            double* init_carr_phase, double carr_step,
+                            double* I_E, double* Q_E,
+                            double* I_P, double* Q_P,
+                            double* I_L, double* Q_L, u32* num_samples);
 
 #endif /* LIBSWIFTNAV_CORRELATE_H */

include/libswiftnav/counter_checker.h

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Adel Mamin <[email protected]>
+ *
+ * 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.
+ */
+
+/**
+  @file
+
+  This utility helps to verify the integrity of samples data from Piksi v3 HW.
+  The regular Piksi v3 samples data format in one byte is:
+  RF4 RF3 RF2 RF1
+   00  00  00  00
+
+  RF1 - GPS L1
+  RF2 - GLONASS L1
+  RF3 - GLONASS L2
+  RF4 - GPS L2
+
+  This utility is expecting RF3 and RF2 to have 4 bits counter,
+  which is continuosly incremented with modulo SAMPLE_COUNTER_MODULO.
+  RF2 is expected to have two least significant bits
+  and RF3 - two most significant bits.
+  Therefore, the bit indexes are:
+  RF3 RF2
+  5&4 3&2
+*/
+
+#ifndef LIBSWIFTNAV_COUNTER_CHECKER_H
+#define LIBSWIFTNAV_COUNTER_CHECKER_H
+
+#include <stdint.h>
+#include <stddef.h>
+#include <libswiftnav/common.h>
+
+/** How many bytes we read from samples stream at a time. */
+#define SAMPLE_CHUNK_SIZE (1024 * 1024) /* [bytes] */
+
+/** The counter embedded into the sample data stream is expected
+    to have this modulo. */
+#define SAMPLE_COUNTER_MODULO 13
+
+/** The counter bit size. */
+#define SAMPLE_COUNTER_BITS   4 /* [bits] */
+
+/** Teh counter bit offset within a byte. */
+#define SAMPLE_COUNTER_OFFSET 2 /* [bits] */
+
+/** How many bytes we read from samples file at a time. */
+#define COUNTER_CHECKER_CHUNK_SIZE (1024 * 1024) /* [bytes] */
+
+/** Get \e num of bits at \e offset in \e data */
+#define GET_BITS(data, offset, num) \
+  ( ((data) >> (offset)) & ((1 << (num)) - 1) )
+
+/** Get \e num of bits at \e offset in \e data */
+#define SET_BITS(data, offset, num, bits) \
+  ( ( ~( ((1 << (num)) - 1) << (offset) ) & (data) ) | \
+  ( ( (bits) & ((1 << (num)) - 1) ) << (offset) ) )
+
+/** Get GPS counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_gps_counter(uint8_t data);
+
+/** Get Glonass counter
+ * \param data Data byte
+ * \return The counter value
+ */
+uint8_t get_glo_counter(uint8_t data);
+
+uint8_t set_gps_counter(uint8_t data, uint8_t counter);
+uint8_t set_glo_counter(uint8_t data, uint8_t counter);
+
+/** A data mismatch descriptor. */
+struct mismatch {
+  size_t offset;                /**! Data offset [bytes]. */
+  uint8_t data;                 /**! Data. */
+  uint8_t expected_counter;     /**! The expected counter value. */
+  uint8_t actual_counter;       /**! The actual counter value. */
+};
+
+/** Mismatch array data */
+struct mismatch_data {
+  /** The sample data counter mismatch incidents are stored here. */
+  struct mismatch data[COUNTER_CHECKER_CHUNK_SIZE];
+  /** How many valid entries there are in \e mismatch array. */
+  size_t counter;
+};
+
+struct callbacks {
+  size_t (*read)(void *ptr, size_t size, void *f);
+  void (*rewind)(void *f);
+  uint8_t (*get_counter)(uint8_t data);
+  uint8_t (*set_counter)(uint8_t data, uint8_t counter);
+};
+
+typedef size_t (*stream_read_t)(void *ptr, size_t size, void *stream);
+typedef void   (*stream_rewind_t)(void *stream);
+
+uint8_t get_rf32_counter(uint8_t data);
+uint8_t set_rf32_counter(uint8_t data, uint8_t counter);
+uint8_t get_rf41_counter(uint8_t data);
+uint8_t set_rf41_counter(uint8_t data, uint8_t counter);
+
+void report_mismatch(const struct mismatch_data *mismatch);
+
+void counter_checker_init(void);
+
+const struct mismatch_data *counter_checker_run(struct callbacks *cbs,
+                                               void *stream,
+                                               size_t data_size);
+
+#endif /* LIBSWIFTNAV_COUNTER_CHECKER_H */

libfec/src/CMakeLists.txt

@@ -11,4 +11,4 @@ add_library(fec viterbi27.c)
 
 install(TARGETS fec DESTINATION lib${LIB_SUFFIX})
 
-install(FILES ${libfec_HEADERS} DESTINATION include/)
+install(FILES ${libfec_HEADERS} DESTINATION include/libfec/)

python/setup.py

@@ -24,26 +24,45 @@
     print "You don't seem to have Cython installed."
     sys.exit(1)
   os.environ['ARCHFLAGS'] = ""
+
+  # Additional library search directories:
+  # - LD_LIBRARY_PATH (if present)
+  # - User local enviroment
+  library_dirs = []
+  # If LD_LIBRARY_PATH has been manually specified, add it to the
+  # library search path
+  if 'LD_LIBRARY_PATH' in os.environ:
+    library_dirs.append(os.environ['LD_LIBRARY_PATH'])
+  library_dirs.append(os.path.expanduser('~/.local/lib'))
+
+  # Additional include directories:
+  # - Numpy includes
+  # - User local enviroment
+  # - Current directory
+  include_dirs = []
+  include_dirs.append(np.get_include())
+  include_dirs.append(os.path.expanduser('~/.local/include'))
+  include_dirs.append('.')
+  # three more includes for travis builds as it does not install libraries
+  include_dirs.append('../include/')
+  include_dirs.append('../libfec/include/')
+  include_dirs.append('../tests/data/l2cbitstream/')
   def make_extension(ext_name):
     ext_path = ext_name.replace('.', os.path.sep) + '.pyx'
-    library_dirs = []
-    # If LD_LIBRARY_PATH has been manually specified, add it to the
-    # library search path
-    if 'LD_LIBRARY_PATH' in os.environ:
-      library_dirs.append(os.environ['LD_LIBRARY_PATH'])
     return Extension(
       ext_name, [ext_path],
-      include_dirs = [np.get_include(), '.', '../include/'],
+      include_dirs = include_dirs,
       extra_compile_args = ['-O0', '-g'],
       extra_link_args = ['-g'],
-      libraries = ['m', 'swiftnav'],
+      libraries = ['m', 'swiftnav', 'l2cbitstream'],
       library_dirs = library_dirs,
     )
   ext_names = [
     'swiftnav.edc',
     'swiftnav.signal',
     'swiftnav.coord_system',
     'swiftnav.constants',
+    'swiftnav.cnav_msg',
     'swiftnav.nav_msg',
     'swiftnav.pvt',
     'swiftnav.correlate',

python/swiftnav/bits.pyx

@@ -12,10 +12,23 @@
 Bit field packing, unpacking and utility functions.
 
 """
-
+from swiftnav.bits cimport parity as c_parity
 
 def parity(x):
-  raise NotImplementedError
+  '''
+  Cython wrapper for parity function
+
+  Parameters
+  ----------
+  x : int
+    Value for parity computation
+
+  Returns
+  -------
+  int
+    Parity value: 1 or 0.
+  '''
+  return c_parity(x)
 
 def getbitu(buff,  pos,  length):
   raise NotImplementedError

python/swiftnav/cnav_msg.pxd

@@ -0,0 +1,36 @@
+# Copyright (C) 2016 Swift Navigation Inc.
+#
+# 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.
+
+from common cimport *
+from libcpp cimport bool
+
+cdef extern from "libswiftnav/cnav_msg.h":
+
+  ctypedef struct cnav_msg_decoder_t:
+    pass
+
+  ctypedef struct cnav_msg_t:
+    u8   prn    # SV PRN. 0..31
+    u8   msg_id # Message id. 0..31
+    u32  tow    # GPS ToW in 6-second units. Multiply to 6 to get seconds. 
+    bool alert  # CNAV message alert flag
+
+  void cnav_msg_decoder_init(cnav_msg_decoder_t *dec)
+  bool cnav_msg_decoder_add_symbol(cnav_msg_decoder_t *dec,
+                                   u8 symbol,
+                                   cnav_msg_t *msg,
+                                   u32 *delay)
+
+cdef extern from "libl2cbitstream/l2cbitstream.h":
+  bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow)
+
+cdef class CNavMsgDecoder:
+  cdef cnav_msg_decoder_t _thisptr
+cdef class CNavMsg:
+  cdef cnav_msg_t _thisptr

python/swiftnav/cnav_msg.pyx

@@ -0,0 +1,95 @@
+# Copyright (C) 2016 Swift Navigation Inc.
+#
+# 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.
+
+from fmt_utils import fmt_repr
+from libc.string cimport memcpy, memset
+cimport numpy as np
+import numpy as np
+
+cdef class CNavMsgDecoder:
+
+  def __init__(self, **kwargs):
+    memset(&self._thisptr, 0, sizeof(cnav_msg_decoder_t))
+    cnav_msg_decoder_init(&self._thisptr)
+
+  def __repr__(self):
+    return fmt_repr(self)
+
+  def decode(self, u8 symbol, CNavMsg msg):
+    cdef u32 delay = 0
+    res = cnav_msg_decoder_add_symbol(&self._thisptr,
+                                      symbol,
+                                      &msg._thisptr,
+                                      &delay)
+    return res, delay
+
+cdef class CNavMsg:
+  def __init__(self, **kwargs):
+    memset(&self._thisptr, 0, sizeof(cnav_msg_t))
+    if 'prn' in kwargs:
+      self._thisptr.prn = kwargs.pop('prn')
+    if 'msg_id' in kwargs:
+      self._thisptr.msg_id = kwargs.pop('msg_id')
+    if 'tow' in kwargs:
+      self._thisptr.tow = kwargs.pop('tow')
+    if 'alert' in kwargs:
+      self._thisptr.alert = kwargs.pop('alert')
+
+  def __getattr__(self, k):
+    return self._thisptr.get(k)
+
+  def __repr__(self):
+    return fmt_repr(self)
+
+  def to_dict(self):
+    return self._thisptr
+
+  def from_dict(self, d):
+    self._thisptr = d
+
+  def __reduce__(self):
+    return (rebuild_CNavMsg, tuple([tuple(self.to_dict().items())]))
+  
+  def getPrn(self):
+    return self._thisptr.prn
+
+  def getTow(self):
+    return self._thisptr.tow
+
+  def getMsgId(self):
+    return self._thisptr.msg_id
+
+  def getAlert(self):
+    return self._thisptr.alert
+
+def rebuild_CNavMsg(reduced):
+  """
+  Rebuild CNavMsg for unpickling.
+
+  Parameters
+  ----------
+  reduced: tuple
+    Tuple of dict of NavMsg cnav_msg_t struct fields
+
+  Returns
+  -------
+  out: :class:`CNavMsg` instance
+    Rebuilt :class:`CNavMsg` instance
+  """
+  nm = CNavMsg()
+  nm.from_dict(dict(reduced))
+  return nm
+
+
+cdef class CNavRawMsg:
+  @staticmethod
+  def generate(prn, msg_id, tow):
+    cdef np.ndarray[np.uint8_t, ndim=1, mode="c"] tmp_ = np.ndarray(38, dtype=np.uint8)
+    res = get_l2c_message(&tmp_[0], prn, msg_id, tow)
+    return np.unpackbits(tmp_)[4:]