From 0619f7362fe2fee33c15284f4063706df5b576fa Mon Sep 17 00:00:00 2001 From: Bhaskar Mookerji Date: Fri, 15 Jan 2016 15:20:57 -0800 Subject: [PATCH 1/4] Print navigation measurements, bug fix, and test. Notably, adds a test case for the make_propagated_sdiffs function with some synthetic data taken from Kleeman's SITL runner. Checks only to see that the produced outputs are non-zero. /cc @akleeman @benjamin0 @fnoble --- python/swiftnav/ambiguity_test.pyx | 2 -- python/swiftnav/ephemeris.pyx | 2 +- python/swiftnav/observation.pyx | 13 ++++--- python/swiftnav/track.pyx | 7 ++++ python/tests/test_observation.py | 57 ++++++++++++++++++++++++++++++ 5 files changed, 73 insertions(+), 8 deletions(-) diff --git a/python/swiftnav/ambiguity_test.pyx b/python/swiftnav/ambiguity_test.pyx index e8698cea..d9c6cc67 100644 --- a/python/swiftnav/ambiguity_test.pyx +++ b/python/swiftnav/ambiguity_test.pyx @@ -137,9 +137,7 @@ cdef class AmbiguityTest: is_bad_measurement): num_sdiffs = len(sdiffs) cdef sdiff_t sdiffs_[32] - print "here" mk_sdiff_array(sdiffs, 32, &sdiffs_[0]) - print sdiffs_ return ambiguity_update_sats(&self._thisptr, num_sdiffs, &sdiffs_[0], diff --git a/python/swiftnav/ephemeris.pyx b/python/swiftnav/ephemeris.pyx index 7ddc165e..35a3dc81 100644 --- a/python/swiftnav/ephemeris.pyx +++ b/python/swiftnav/ephemeris.pyx @@ -82,4 +82,4 @@ cdef mk_ephemeris_array(py_ephemerides, u8 n_c_ephemerides, """ for (i, ephemeris) in enumerate(py_ephemerides): - c_ephemerides[i] = &((ephemeris[i])._thisptr) + c_ephemerides[i] = &((ephemeris)._thisptr) diff --git a/python/swiftnav/observation.pyx b/python/swiftnav/observation.pyx index b1e6c7e4..b901a337 100644 --- a/python/swiftnav/observation.pyx +++ b/python/swiftnav/observation.pyx @@ -43,6 +43,9 @@ cdef class SingleDiff: def from_dict(self, d): self._thisptr = d + def __rich_cmp__(self, SingleDiff sd, op): + return cmp_sdiff(&self._thisptr, &sd._thisptr) + def single_diff_(m_a, m_b): """Calculate single differences from two sets of observations. Undifferenced input observations are assumed to be both taken at the @@ -214,10 +217,10 @@ cdef mk_sdiff_array(py_sdiffs, u8 n_c_sdiffs, sdiff_t *c_sdiffs): cdef read_sdiff_array(u8 n_c_sdiffs, sdiff_t *c_sdiffs): """Given an array of c sdiff_t's, returns an array of SingleDiffs. + Not efficient, but works for now. + """ - cdef sdiff_t sd_ - py_sdiffs = [SingleDiff()]*n_c_sdiffs - for (i, sdiff) in enumerate(py_sdiffs): - sd_ = ( sdiff)._thisptr - memcpy(&sd_, &c_sdiffs[i], sizeof(sdiff_t)) + py_sdiffs = [] + for n in range(n_c_sdiffs): + py_sdiffs.append(SingleDiff(**c_sdiffs[n])) return py_sdiffs diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index 0a60b21b..1cc7e074 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -18,6 +18,7 @@ from common cimport * from ephemeris cimport ephemeris_t from ephemeris cimport Ephemeris from ephemeris import Ephemeris +from fmt_utils import fmt_repr from time cimport * from time import GpsTime from libc.stdlib cimport malloc, free @@ -453,6 +454,12 @@ cdef class NavigationMeasurement: def __getattr__(self, k): return self._thisptr.get(k) + def to_dict(self): + return self._thisptr + + def __repr__(self): + return fmt_repr(self) + def __rich_cmp__(self, nav_msg, op): cdef navigation_measurement_t nav_msg_ = nav_msg._thisptr return nav_meas_cmp((&self._thisptr), (&nav_msg_)) diff --git a/python/tests/test_observation.py b/python/tests/test_observation.py index db954540..73ff800c 100644 --- a/python/tests/test_observation.py +++ b/python/tests/test_observation.py @@ -9,6 +9,7 @@ # EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. +from swiftnav.ephemeris import Ephemeris from swiftnav.pvt import calc_PVT_ import numpy as np import pytest @@ -133,3 +134,59 @@ def test_single_diff(): assert len(sdiffs) == 2 sdiffs = o.single_diff_([nms[1], nms[0]], [nms[2], nms[0]]) assert len(sdiffs) == 1 + +def test_prop_single_diff(): + rover_nm = [t.NavigationMeasurement(sat_pos=[-19277207.52067729, -8215764.2479763795, 16367744.770246204], + pseudorange=21123480.27105955, doppler=0, raw_doppler=0, carrier_phase=-110993309.26669005, + sat_vel=[-1025.0370403901404, -1821.9217799467374, -2091.6303199092254], + lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.92954684512), + raw_pseudorange=21121324.476403236, + snr=30.0, sid=s.GNSSSignal(band=0, constellation=0, sat=0), lock_counter=0), + t.NavigationMeasurement(sat_pos=[-16580310.849158794, 918714.1939749047, 20731444.258332774], + pseudorange=22432049.84763688, doppler=0, raw_doppler=0, carrier_phase=-117882743.21601027, + sat_vel=[1060.9864205977192, -2411.43509917502, 953.6270954519971], + lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.9251737675), + raw_pseudorange=22432340.166121125, + snr=30.0, sid=s.GNSSSignal(band=0, constellation=0, sat=2), lock_counter=0)] + remote_dists = np.array([ 21121393.87562408, 22432814.46819838]) + base_pos = np.array( [-2704375, -4263211, 3884637]) + es = [Ephemeris(**{'ura': 0.0, 'healthy': 1, + 'xyz': {'acc': [-6.51925802230835e-08, 4.718410826464475e-09, 2.6226835183928943], + 'iod': 0, 'a_gf0': 5153.648313522339, 'a_gf1': 0.5977821557062277, + 'pos': [5.122274160385132e-09, 19.0625, 259.9375], + 'rate': [1.0151416063308716e-06, 6.260350346565247e-06, -6.332993507385254e-08], + 'toa': 4096}, + 'valid': 1, + 'sid': {'band': 0, 'constellation': 0, 'sat': 0}, + 'toe': {'wn': 1876, 'tow': 172800.0}, + 'kepler': {'inc_dot': 4.4966158735287064e-10, 'tgd': 5.122274160385132e-09, 'omegadot': -8.099980253833005e-09, + 'sqrta': 5153.648313522339, 'inc': 0.9634151551139846, 'cus': 6.260350346565247e-06, + 'omega0': 0.5977821557062277, 'cuc': 1.0151416063308716e-06, 'm0': 2.6226835183928943, + 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.718410826464475e-09, 'ecc': 0.0049016030970960855, + 'cic': -6.332993507385254e-08, 'crs': 19.0625, 'iode': 74, 'iodc': 21845, 'cis': -6.51925802230835e-08, + 'crc': 259.9375, 'w': 0.4885959643259506, 'af0': 7.212162017822266e-06, 'af1': 9.094947017729282e-13, 'af2': 0.0}, + 'fit_interval': 4}), + Ephemeris(**{'ura': 0.0, 'healthy': 1, + 'xyz': {'acc': [-6.146728992462158e-08, 4.742340394655613e-09, -0.8114190126645531], 'iod': 0, + 'a_gf0': 5153.798839569092, 'a_gf1': 1.6390180338742641, + 'pos': [1.862645149230957e-09, -40.5625, 221.625], + 'rate': [-2.2239983081817627e-06, 8.001923561096191e-06, 3.725290298461914e-09], 'toa': 0}, + 'valid': 1, + 'sid': {'band': 0, 'constellation': 0, 'sat': 2}, + 'toe': {'wn': 1876, 'tow': 172800.0}, + 'kepler': {'inc_dot': -4.475186409476941e-10, 'tgd': 1.862645149230957e-09, 'omegadot': -8.103551831174965e-09, + 'sqrta': 5153.798839569092, 'inc': 0.9587534715647247, 'cus': 8.001923561096191e-06, + 'omega0': 1.6390180338742641, 'cuc': -2.2239983081817627e-06, 'm0': -0.8114190126645531, + 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.742340394655613e-09, 'ecc': 0.00035172002390027046, + 'cic': 3.725290298461914e-09, 'crs': -40.5625, 'iode': 59, 'iodc': 21845, 'cis': -6.146728992462158e-08, + 'crc': 221.625, 'w': 2.9037284161724037, 'af0': -9.969808161258698e-07, 'af1': -5.229594535194337e-12, 'af2': 0.0}, + 'fit_interval': 4})] + gpst = ti.GpsTime(wn=1876, tow=167132) + sdiffs = o.make_propagated_sdiffs_(rover_nm, rover_nm, remote_dists, base_pos, es, gpst) + assert len(sdiffs) == 2 + assert sdiffs[0].pseudorange > 0 + assert sdiffs[1].pseudorange > 0 + assert sdiffs[0].carrier_phase < 0 + assert sdiffs[0].doppler > 0 + assert sdiffs[0].sid['sat'] == 0 + assert sdiffs[1].sid['sat'] == 2 From 50ef678759e8d14b3db8eeec3870639cc8b3883a Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Wed, 20 Jan 2016 14:20:50 +0200 Subject: [PATCH 2/4] Add Convolutional encoder 2,7 Convolutional encoder is python program that takes any input file to encode it into output symbol stream. --- tests/data/v27_enc.py | 128 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 128 insertions(+) create mode 100644 tests/data/v27_enc.py diff --git a/tests/data/v27_enc.py b/tests/data/v27_enc.py new file mode 100644 index 00000000..f92f0033 --- /dev/null +++ b/tests/data/v27_enc.py @@ -0,0 +1,128 @@ +''' +Copyright (C) 2016 Swift Navigation Inc. +Contact: Dmitry Tatarinov + +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. + +''' + +''' +The Python script performs convolutional encoding of a file for Viterbi +decoder testing purposes. + +USAGE: + python v27_enc.py + +OUTPUT: + Symbol file which has encoded data, each byte is + represented as 0xff for '1' and 0x00 for '0' parity bits. +''' + +import struct +import os +import sys + +G1 = 0171 #generator polinomial for p1 +G2 = 0133 #generator polinomial for p2 + +def parity(st): + return (0x6996 >> ((st ^ (st >> 4)) & 15)) & 1 + +state = 0 + +if len(sys.argv) < 3: + print 'Not enough input data. Usage: v27_enc.py ' + exit() +else: + f_in_name = str(sys.argv[1]) + f_out_name = str(sys.argv[2]) + +f_out_hard = open(f_out_name,'wb') +f_in = open(f_in_name,'rb') + +f_in_size = os.path.getsize(f_in_name) + +byte = 0 + +pre = [0,0,0,0] +post = [0,0,0,0] + +# 2 bits delay to have data be byte aligned +j = 3 +while j >= 0: + f_out_hard.write(chr(0)) + j -= 1 + +#encode prefix first +for b in pre: + j = 7 + while j >= 0: + state = (((b >> j) & 1) << 6) | (state >> 1) + p1 = parity(state & G1) + p2 = parity(state & G2) + #here is output SN-like for Viterbi decoder testing + if p1 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + if p2 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + j -= 1 + +#now encode data from file +with open(f_in_name,'rb') as f_in: + while f_in_size: + byte = struct.unpack('B',f_in.read(1))[0] #read byte from input file + + j = 7 + while j >= 0: #for each bit in the byte + state = (((byte >> j) & 1) << 6) | (state >> 1) + p1 = parity(state & G1) #calculate parity + p2 = parity(state & G2) + + #here is output SN-like for Viterbi decoder testing + if p1 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + if p2 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + j -= 1 + + f_in_size -= 1; + +#encode postfix +for b in post: + j = 7 + while j >= 0: + state = (((b >> j) & 1) << 6) | (state >> 1) + p1 = parity(state & G1) + p2 = parity(state & G2) + #here is output SN-like for Viterbi decoder testing + if p1 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + if p2 == 1: + f_out_hard.write(chr(0xff)) + else: + f_out_hard.write(chr(0)) + + j -= 1 + +f_out_hard.close() +f_in.close() From ebbcfc401f71746155a5c788510d860cbb08f861 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Wed, 27 Jan 2016 20:38:12 +0200 Subject: [PATCH 3/4] Remove accidentially rebased commit 0619f73 --- python/swiftnav/ambiguity_test.pyx | 2 ++ python/swiftnav/ephemeris.pyx | 2 +- python/swiftnav/observation.pyx | 13 +++---- python/swiftnav/track.pyx | 7 ---- python/tests/test_observation.py | 57 ------------------------------ 5 files changed, 8 insertions(+), 73 deletions(-) diff --git a/python/swiftnav/ambiguity_test.pyx b/python/swiftnav/ambiguity_test.pyx index d9c6cc67..e8698cea 100644 --- a/python/swiftnav/ambiguity_test.pyx +++ b/python/swiftnav/ambiguity_test.pyx @@ -137,7 +137,9 @@ cdef class AmbiguityTest: is_bad_measurement): num_sdiffs = len(sdiffs) cdef sdiff_t sdiffs_[32] + print "here" mk_sdiff_array(sdiffs, 32, &sdiffs_[0]) + print sdiffs_ return ambiguity_update_sats(&self._thisptr, num_sdiffs, &sdiffs_[0], diff --git a/python/swiftnav/ephemeris.pyx b/python/swiftnav/ephemeris.pyx index 35a3dc81..7ddc165e 100644 --- a/python/swiftnav/ephemeris.pyx +++ b/python/swiftnav/ephemeris.pyx @@ -82,4 +82,4 @@ cdef mk_ephemeris_array(py_ephemerides, u8 n_c_ephemerides, """ for (i, ephemeris) in enumerate(py_ephemerides): - c_ephemerides[i] = &((ephemeris)._thisptr) + c_ephemerides[i] = &((ephemeris[i])._thisptr) diff --git a/python/swiftnav/observation.pyx b/python/swiftnav/observation.pyx index b901a337..b1e6c7e4 100644 --- a/python/swiftnav/observation.pyx +++ b/python/swiftnav/observation.pyx @@ -43,9 +43,6 @@ cdef class SingleDiff: def from_dict(self, d): self._thisptr = d - def __rich_cmp__(self, SingleDiff sd, op): - return cmp_sdiff(&self._thisptr, &sd._thisptr) - def single_diff_(m_a, m_b): """Calculate single differences from two sets of observations. Undifferenced input observations are assumed to be both taken at the @@ -217,10 +214,10 @@ cdef mk_sdiff_array(py_sdiffs, u8 n_c_sdiffs, sdiff_t *c_sdiffs): cdef read_sdiff_array(u8 n_c_sdiffs, sdiff_t *c_sdiffs): """Given an array of c sdiff_t's, returns an array of SingleDiffs. - Not efficient, but works for now. - """ - py_sdiffs = [] - for n in range(n_c_sdiffs): - py_sdiffs.append(SingleDiff(**c_sdiffs[n])) + cdef sdiff_t sd_ + py_sdiffs = [SingleDiff()]*n_c_sdiffs + for (i, sdiff) in enumerate(py_sdiffs): + sd_ = ( sdiff)._thisptr + memcpy(&sd_, &c_sdiffs[i], sizeof(sdiff_t)) return py_sdiffs diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index 1cc7e074..0a60b21b 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -18,7 +18,6 @@ from common cimport * from ephemeris cimport ephemeris_t from ephemeris cimport Ephemeris from ephemeris import Ephemeris -from fmt_utils import fmt_repr from time cimport * from time import GpsTime from libc.stdlib cimport malloc, free @@ -454,12 +453,6 @@ cdef class NavigationMeasurement: def __getattr__(self, k): return self._thisptr.get(k) - def to_dict(self): - return self._thisptr - - def __repr__(self): - return fmt_repr(self) - def __rich_cmp__(self, nav_msg, op): cdef navigation_measurement_t nav_msg_ = nav_msg._thisptr return nav_meas_cmp((&self._thisptr), (&nav_msg_)) diff --git a/python/tests/test_observation.py b/python/tests/test_observation.py index 73ff800c..db954540 100644 --- a/python/tests/test_observation.py +++ b/python/tests/test_observation.py @@ -9,7 +9,6 @@ # EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. -from swiftnav.ephemeris import Ephemeris from swiftnav.pvt import calc_PVT_ import numpy as np import pytest @@ -134,59 +133,3 @@ def test_single_diff(): assert len(sdiffs) == 2 sdiffs = o.single_diff_([nms[1], nms[0]], [nms[2], nms[0]]) assert len(sdiffs) == 1 - -def test_prop_single_diff(): - rover_nm = [t.NavigationMeasurement(sat_pos=[-19277207.52067729, -8215764.2479763795, 16367744.770246204], - pseudorange=21123480.27105955, doppler=0, raw_doppler=0, carrier_phase=-110993309.26669005, - sat_vel=[-1025.0370403901404, -1821.9217799467374, -2091.6303199092254], - lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.92954684512), - raw_pseudorange=21121324.476403236, - snr=30.0, sid=s.GNSSSignal(band=0, constellation=0, sat=0), lock_counter=0), - t.NavigationMeasurement(sat_pos=[-16580310.849158794, 918714.1939749047, 20731444.258332774], - pseudorange=22432049.84763688, doppler=0, raw_doppler=0, carrier_phase=-117882743.21601027, - sat_vel=[1060.9864205977192, -2411.43509917502, 953.6270954519971], - lock_time=0, tot=ti.GpsTime(wn=1876, tow=167131.9251737675), - raw_pseudorange=22432340.166121125, - snr=30.0, sid=s.GNSSSignal(band=0, constellation=0, sat=2), lock_counter=0)] - remote_dists = np.array([ 21121393.87562408, 22432814.46819838]) - base_pos = np.array( [-2704375, -4263211, 3884637]) - es = [Ephemeris(**{'ura': 0.0, 'healthy': 1, - 'xyz': {'acc': [-6.51925802230835e-08, 4.718410826464475e-09, 2.6226835183928943], - 'iod': 0, 'a_gf0': 5153.648313522339, 'a_gf1': 0.5977821557062277, - 'pos': [5.122274160385132e-09, 19.0625, 259.9375], - 'rate': [1.0151416063308716e-06, 6.260350346565247e-06, -6.332993507385254e-08], - 'toa': 4096}, - 'valid': 1, - 'sid': {'band': 0, 'constellation': 0, 'sat': 0}, - 'toe': {'wn': 1876, 'tow': 172800.0}, - 'kepler': {'inc_dot': 4.4966158735287064e-10, 'tgd': 5.122274160385132e-09, 'omegadot': -8.099980253833005e-09, - 'sqrta': 5153.648313522339, 'inc': 0.9634151551139846, 'cus': 6.260350346565247e-06, - 'omega0': 0.5977821557062277, 'cuc': 1.0151416063308716e-06, 'm0': 2.6226835183928943, - 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.718410826464475e-09, 'ecc': 0.0049016030970960855, - 'cic': -6.332993507385254e-08, 'crs': 19.0625, 'iode': 74, 'iodc': 21845, 'cis': -6.51925802230835e-08, - 'crc': 259.9375, 'w': 0.4885959643259506, 'af0': 7.212162017822266e-06, 'af1': 9.094947017729282e-13, 'af2': 0.0}, - 'fit_interval': 4}), - Ephemeris(**{'ura': 0.0, 'healthy': 1, - 'xyz': {'acc': [-6.146728992462158e-08, 4.742340394655613e-09, -0.8114190126645531], 'iod': 0, - 'a_gf0': 5153.798839569092, 'a_gf1': 1.6390180338742641, - 'pos': [1.862645149230957e-09, -40.5625, 221.625], - 'rate': [-2.2239983081817627e-06, 8.001923561096191e-06, 3.725290298461914e-09], 'toa': 0}, - 'valid': 1, - 'sid': {'band': 0, 'constellation': 0, 'sat': 2}, - 'toe': {'wn': 1876, 'tow': 172800.0}, - 'kepler': {'inc_dot': -4.475186409476941e-10, 'tgd': 1.862645149230957e-09, 'omegadot': -8.103551831174965e-09, - 'sqrta': 5153.798839569092, 'inc': 0.9587534715647247, 'cus': 8.001923561096191e-06, - 'omega0': 1.6390180338742641, 'cuc': -2.2239983081817627e-06, 'm0': -0.8114190126645531, - 'toc': {'wn': 1876, 'tow': 172800.0}, 'dn': 4.742340394655613e-09, 'ecc': 0.00035172002390027046, - 'cic': 3.725290298461914e-09, 'crs': -40.5625, 'iode': 59, 'iodc': 21845, 'cis': -6.146728992462158e-08, - 'crc': 221.625, 'w': 2.9037284161724037, 'af0': -9.969808161258698e-07, 'af1': -5.229594535194337e-12, 'af2': 0.0}, - 'fit_interval': 4})] - gpst = ti.GpsTime(wn=1876, tow=167132) - sdiffs = o.make_propagated_sdiffs_(rover_nm, rover_nm, remote_dists, base_pos, es, gpst) - assert len(sdiffs) == 2 - assert sdiffs[0].pseudorange > 0 - assert sdiffs[1].pseudorange > 0 - assert sdiffs[0].carrier_phase < 0 - assert sdiffs[0].doppler > 0 - assert sdiffs[0].sid['sat'] == 0 - assert sdiffs[1].sid['sat'] == 2 From b5fec65ae9eedbfaf05b1bac37bb0410d9e42559 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Fri, 29 Jan 2016 15:53:08 +0200 Subject: [PATCH 4/4] Move v27_enc.py to tests/scripts folder --- tests/{data => scripts}/v27_enc.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename tests/{data => scripts}/v27_enc.py (100%) diff --git a/tests/data/v27_enc.py b/tests/scripts/v27_enc.py similarity index 100% rename from tests/data/v27_enc.py rename to tests/scripts/v27_enc.py