Merge pull request #14 from fnoble/pool_bindings

Ambiguity test beindings to expose pool as iterator

swiftnav/ambiguity_test.pxd

@@ -0,0 +1,14 @@
+# 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 ambiguity_test_c
+
+cdef class AmbiguityTest:
+  cdef ambiguity_test_c.ambiguity_test_t *test
+

swiftnav/ambiguity_test.pyx

@@ -8,10 +8,25 @@
 # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
 
 cimport ambiguity_test_c
+cimport memory_pool_c
 import numpy as np
 cimport numpy as np
 from common cimport *
 
+cdef hypothesis_to_tuple(u8 num_dds, ambiguity_test_c.hypothesis_t *h):
+  ambs = [ h.N[i] for i in range(num_dds) ]
+  return (h.ll, ambs)
+
+cdef class AmbiguityTest:
+
+  def __iter__(self):
+    cdef memory_pool_c.node_t *currp = self.test.pool.allocated_nodes_head
+    while currp is not NULL:
+      yield hypothesis_to_tuple(self.test.sats.num_sats - 1,
+              <ambiguity_test_c.hypothesis_t *>currp.elem)
+      currp = currp.hdr.next
+
+
 def get_phase_obs_null_basis(DE):
   num_dds = DE.shape[0]
   cdef np.ndarray[np.double_t, ndim=2, mode="c"] DE_ = \
@@ -43,7 +58,7 @@ cdef class ResidualMtxs:
   cdef u8 num_dds
 
   def __init__(self,
-               np.ndarray[np.double_t, ndim=2, mode="c"] DE_mtx, 
+               np.ndarray[np.double_t, ndim=2, mode="c"] DE_mtx,
                np.ndarray[np.double_t, ndim=2, mode="c"] obs_cov):
 
     self.num_dds = obs_cov.shape[0] / 2
@@ -72,4 +87,4 @@ cdef class ResidualMtxs:
 
 # void assign_r_vec(residual_mtxs_t *res_mtxs, u8 num_dds, double *dd_measurements, double *r_vec)
 #   void assign_r_mean(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothesis, double *r_mean)
-#   double get_quadratic_term(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothesis, double *r_vec)
+#   double get_quadratic_term(residual_mtxs_t *res_mtxs, u8 num_dds, double *hypothesis, double *r_vec)

swiftnav/ambiguity_test_c.pxd

@@ -9,14 +9,28 @@
 
 
 from common cimport *
+cimport memory_pool_c
+cimport sats_management_c
+from constants_c cimport MAX_CHANNELS
 
 cdef extern from "libswiftnav/ambiguity_test.h":
+
+  ctypedef struct hypothesis_t:
+    s32 N[MAX_CHANNELS-1]
+    float ll
+
   ctypedef struct residual_mtxs_t:
     u32 res_dim
     u8 null_space_dim
     double *null_projector
     double *half_res_cov_inv
 
+  ctypedef struct ambiguity_test_t:
+    u8 num_dds
+    memory_pool_c.memory_pool_t *pool
+    sats_management_c.sats_management_t sats
+    residual_mtxs_t res_mtxs
+
   void assign_phase_obs_null_basis(u8 num_dds, double *DE_mtx, double *q)
   void assign_residual_covariance_inverse(u8 num_dds, double *obs_cov, double *q, double *r_cov_inv)
   void init_residual_matrices(residual_mtxs_t *res_mtxs, u8 num_dds, double *DE_mtx, double *obs_cov)

swiftnav/constants_c.pxd

@@ -0,0 +1,17 @@
+# 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/constants.h":
+  # HACK: This is declared as an enum so that Cython knows its a constant
+  # https://groups.google.com/d/msg/cython-users/-fLG08E5lYM/xC93UHSvLF0J
+  enum: MAX_CHANNELS
+

swiftnav/dgnss_management.pyx

@@ -23,6 +23,9 @@ from gpstime_c cimport *
 from libc.string cimport memcpy
 from libc.stdio cimport printf
 from sats_management_c cimport *
+from ambiguity_test_c cimport *
+from ambiguity_test import AmbiguityTest
+from ambiguity_test cimport AmbiguityTest
 
 
 def set_settings(phase_var_test, code_var_test,
@@ -274,4 +277,10 @@ def dgnss_iar_MLE_ambs():
   cdef np.ndarray[np.int32_t, ndim=1, mode="c"] ambs = \
     np.empty(32, dtype=np.int32)
   num_dds = dgnss_management_c.dgnss_iar_MLE_ambs(&ambs[0])
-  return ambs[:num_dds]
+  return ambs[:num_dds]
+
+def get_ambiguity_test():
+  test = AmbiguityTest()
+  test.test = dgnss_management_c.get_ambiguity_test()
+  return test
+

swiftnav/dgnss_management_c.pxd

@@ -11,6 +11,7 @@ from common cimport *
 from single_diff_c cimport *
 from amb_kf_c cimport *
 from sats_management_c cimport *
+cimport ambiguity_test_c
 
 cdef extern from "libswiftnav/dgnss_management.h":
   void dgnss_set_settings(double phase_var_test, double code_var_test,
@@ -51,4 +52,5 @@ cdef extern from "libswiftnav/dgnss_management.h":
   u8 get_amb_kf_cov(double *cov)
   u8 get_amb_kf_prns(u8 *prns)
   u8 get_amb_test_prns(u8 *prns)
-  u8 dgnss_iar_MLE_ambs(s32 *ambs)
+  u8 dgnss_iar_MLE_ambs(s32 *ambs)
+  ambiguity_test_c.ambiguity_test_t* get_ambiguity_test()

swiftnav/memory_pool_c.pxd

@@ -0,0 +1,25 @@
+# Copyright (C) 2015 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/memory_pool.h":
+
+  ctypedef u8 element_t
+
+  ctypedef struct memory_pool_node_hdr_t:
+    node_t *next
+
+  ctypedef struct node_t:
+    memory_pool_node_hdr_t hdr
+    element_t elem[0]
+
+  ctypedef struct memory_pool_t:
+    node_t *allocated_nodes_head
+