tracking new files

swiftnav/almanac.pxd

@@ -0,0 +1,13 @@
+# Copyright (C) 2014 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.
+
+cimport almanac_c
+
+cdef class Almanac:
+  cdef almanac_c.almanac_t almanac

swiftnav/dgnss_management.pyx

@@ -0,0 +1,112 @@
+# Copyright (C) 2014 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.
+
+import numpy as np
+cimport numpy as np
+cimport dgnss_management_c
+from float_kf import KalmanFilter
+from float_kf cimport *
+from float_kf_c cimport *
+from single_diff_c cimport *
+from almanac cimport *
+from almanac_c cimport *
+from gpstime cimport *
+from gpstime_c cimport *
+from libc.string cimport memcpy
+from libc.stdio cimport printf
+from sats_management_c cimport *
+
+def dgnss_init(alms, GpsTime timestamp,
+               numpy_measurements,
+               reciever_ecef, dt):
+  n = len(alms)
+  state_dim = n + 5
+  obs_dim = 2 * (n-1)
+
+  cdef almanac_t al[32]
+  cdef almanac_t a_
+  cdef np.ndarray[np.uint8_t, ndim=1, mode="c"] prns = \
+        np.empty(n, dtype=np.uint8)
+  for i, a in enumerate(alms):
+    a_ = (<Almanac> a).almanac
+    memcpy(&al[i], &a_, sizeof(almanac_t))
+    prns[i] = (<Almanac> a).prn
+
+  cdef np.ndarray[np.double_t, ndim=1, mode="c"] ref_ecef_ = \
+    np.array(reciever_ecef, dtype=np.double)
+
+  cdef gps_time_t timestamp_ = timestamp.gps_time
+
+  cdef sdiff_t sdiffs[32]
+  almanacs_to_single_diffs(len(alms), &al[0], timestamp_, sdiffs)
+
+  for i, (l,c) in enumerate(numpy_measurements):
+    sdiffs[i].pseudorange = c
+    sdiffs[i].carrier_phase = l
+
+  dgnss_management_c.dgnss_init(n, &sdiffs[0], &ref_ecef_[0], dt)
+
+def dgnss_update(alms, GpsTime timestamp,
+               numpy_measurements,
+               reciever_ecef, dt):
+  n = len(alms)
+  state_dim = n + 5
+  obs_dim = 2 * (n-1)
+
+  cdef almanac_t al[32]
+  cdef almanac_t a_
+  cdef np.ndarray[np.uint8_t, ndim=1, mode="c"] prns = \
+        np.empty(n, dtype=np.uint8)
+  for i, a in enumerate(alms):
+    a_ = (<Almanac> a).almanac
+    memcpy(&al[i], &a_, sizeof(almanac_t))
+    prns[i] = (<Almanac> a).prn
+
+  cdef np.ndarray[np.double_t, ndim=1, mode="c"] ref_ecef_ = \
+    np.array(reciever_ecef, dtype=np.double)
+
+  cdef gps_time_t timestamp_ = timestamp.gps_time
+
+  cdef sdiff_t sdiffs[32]
+  almanacs_to_single_diffs(len(alms), &al[0], timestamp_, sdiffs)
+
+  for i, (l,c) in enumerate(numpy_measurements):
+    sdiffs[i].pseudorange = c
+    sdiffs[i].carrier_phase = l
+
+  dgnss_management_c.dgnss_update(n, &sdiffs[0], &ref_ecef_[0], dt)
+
+def get_dgnss_kf():
+  cdef kf_t *kf = dgnss_management_c.get_dgnss_kf()
+  cdef KalmanFilter pykf = KalmanFilter()
+  memcpy(&(pykf.kf), kf, sizeof(float_kf_c.kf_t))
+  return pykf
+
+
+def get_stupid_state(num):
+  cdef np.ndarray[np.int32_t, ndim=1, mode="c"] N = \
+    np.empty(num, dtype=np.int32)
+  cdef s32 * c_N = dgnss_management_c.get_stupid_filter_ints()
+  memcpy(&N[0], c_N, num * sizeof(s32))
+  return N
+
+def get_sats_management():
+  cdef sats_management_t * sats_man = dgnss_management_c.get_sats_management()
+  cdef np.ndarray[np.uint8_t, ndim=1, mode="c"] prns = \
+    np.empty(sats_man.num_sats, dtype=np.uint8)
+  memcpy(&prns[0], sats_man.prns, sats_man.num_sats * sizeof(u8))
+  return sats_man.num_sats, prns
+
+
+
+
+
+
+
+

swiftnav/dgnss_management_c.pxd

@@ -0,0 +1,21 @@
+# Copyright (C) 2014 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 single_diff_c cimport *
+from float_kf_c cimport *
+from sats_management_c cimport *
+
+cdef extern from "libswiftnav/dgnss_management.h":
+  void make_measurements(u8 num_diffs, sdiff_t *sdiffs, double *raw_measurements)
+  void dgnss_init(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3], double dt)
+  void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3], double dt)
+  kf_t * get_dgnss_kf()
+  s32 * get_stupid_filter_ints()
+  sats_management_t * get_sats_management()

swiftnav/float_kf.pxd

@@ -0,0 +1,13 @@
+# Copyright (C) 2014 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.
+
+cimport float_kf_c
+
+cdef class KalmanFilter:
+  cdef float_kf_c.kf_t kf

swiftnav/gpstime.pxd

@@ -0,0 +1,13 @@
+# Copyright (C) 2014 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.
+
+cimport gpstime_c
+
+cdef class GpsTime:
+  cdef gpstime_c.gps_time_t gps_time

swiftnav/gpstime.pyx

@@ -0,0 +1,33 @@
+# Copyright (C) 2014 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.
+
+cimport gpstime_c
+
+cdef class GpsTime:
+  cdef gpstime_c.gps_time_t gps_time
+  def __init__(self,
+				       wn,
+               tow):
+    self.wn = wn
+    self.tow = tow
+
+  def __repr__(self):
+    return "<GpsTime wn " + str(self.wn) + ", tow " + str(self.tow) + ">"
+
+  property wn:
+    def __get__(self):
+      return self.gps_time.wn
+    def __set__(self, wn):
+      self.gps_time.wn = wn
+
+  property tow:
+    def __get__(self):
+      return self.gps_time.tow
+    def __set__(self, tow):
+      self.gps_time.tow = tow

swiftnav/sats_management_c.pxd

@@ -0,0 +1,15 @@
+# Copyright (C) 2014 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 *
+
+cdef extern from "libswiftnav/sats_management.h":
+  ctypedef struct sats_management_t:
+    u8 num_sats
+    u8 * prns

swiftnav/single_diff.pyx

@@ -0,0 +1,52 @@
+# Copyright (C) 2014 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.
+
+import numpy as np
+cimport numpy as np
+cimport single_diff_c
+from libc.string cimport memcpy
+
+cdef class SingleDiff:
+  cdef single_diff_c.sdiff_t sdiff
+  def __init__(self,
+               np.ndarray[np.double_t, ndim=1, mode="c"] sat_pos, 
+               np.ndarray[np.double_t, ndim=1, mode="c"] sat_vel,
+               prn):
+    self.sat_pos = sat_pos
+    self.sat_vel = sat_vel
+    self.prn = prn
+
+  def __repr__(self):
+    return "<SingleDiff prn=" + str(self.prn) + ">"
+
+  property sat_pos:
+    def __get__(self):
+      cdef np.ndarray[np.double_t, ndim=1, mode="c"] sat_pos = \
+        np.empty(3, dtype=np.double)
+      memcpy(&sat_pos[0], self.sdiff.sat_pos, 3 * sizeof(double))
+      return sat_pos
+    def __set__(self, np.ndarray[np.double_t, ndim=1, mode="c"] sat_pos):
+      memcpy(self.sdiff.sat_pos, &sat_pos[0], 3 * sizeof(double))
+
+  property sat_vel:
+    def __get__(self):
+      cdef np.ndarray[np.double_t, ndim=1, mode="c"] sat_vel = \
+        np.empty(3, dtype=np.double)
+      memcpy(&sat_vel[0], self.sdiff.sat_vel, 3 * sizeof(double))
+      return sat_vel
+    def __set__(self, np.ndarray[np.double_t, ndim=1, mode="c"] sat_vel):
+      memcpy(self.sdiff.sat_vel, &sat_vel[0], 3 * sizeof(double))
+
+  property prn:
+    def __get__(self):
+      return self.sdiff.prn
+    def __set__(self, prn):
+      self.sdiff.prn = prn
+
+

swiftnav/single_diff_c.pxd

@@ -0,0 +1,24 @@
+# Copyright (C) 2014 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 almanac_c cimport *
+from gpstime_c cimport *
+
+cdef extern from "libswiftnav/single_diff.h":
+  ctypedef struct sdiff_t:
+    double pseudorange
+    double carrier_phase
+    double doppler
+    double *sat_pos
+    double *sat_vel
+    double snr
+    u8 prn
+
+  void almanacs_to_single_diffs(u8 n, almanac_t *alms, gps_time_t timestamp, sdiff_t *sdiffs)