From 5f94af51a4cd916e340634c9eaeaf69c92ffad87 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Sat, 6 Feb 2016 11:43:08 +0200 Subject: [PATCH 01/18] Add L2C correlator for Peregrine --- include/libswiftnav/correlate.h | 25 +- python/swiftnav/correlate.pxd | 13 +- python/swiftnav/correlate.pyx | 13 +- src/correlate.c | 331 ++++++++++++++++++++--- tests/CMakeLists.txt | 1 + tests/check_correlator.c | 462 ++++++++++++++++++++++++++++++++ tests/check_main.c | 1 + tests/check_suites.h | 1 + 8 files changed, 795 insertions(+), 52 deletions(-) create mode 100644 tests/check_correlator.c diff --git a/include/libswiftnav/correlate.h b/include/libswiftnav/correlate.h index 7bacc09d..14cebfd3 100644 --- a/include/libswiftnav/correlate.h +++ b/include/libswiftnav/correlate.h @@ -1,6 +1,7 @@ /* - * Copyright (C) 2013 Swift Navigation Inc. + * Copyright (C) 2013,2016 Swift Navigation Inc. * Contact: Fergus Noble + * Contact: Adel Mamin * * 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,20 @@ #include -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, + 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, + 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 */ diff --git a/python/swiftnav/correlate.pxd b/python/swiftnav/correlate.pxd index c3ee29d0..a593d427 100644 --- a/python/swiftnav/correlate.pxd +++ b/python/swiftnav/correlate.pxd @@ -1,4 +1,5 @@ -# Copyright (C) 2015 Swift Navigation Inc. +# Copyright (C) 2015,2016 Swift Navigation Inc. +# Contact: Adel Mamin # # 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. @@ -10,7 +11,15 @@ from common cimport * cdef extern from "libswiftnav/correlate.h": - void track_correlate(s8* samples, s8* code, + void l1_ca_track_correlate(s8* samples, size_t samples_len, 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 l2c_cm_track_correlate(s8* samples, size_t samples_len, s8* code, double* init_code_phase, double code_step, double* init_carr_phase, double carr_step, double* I_E, double* Q_E, diff --git a/python/swiftnav/correlate.pyx b/python/swiftnav/correlate.pyx index 2c75e57c..8de52071 100644 --- a/python/swiftnav/correlate.pyx +++ b/python/swiftnav/correlate.pyx @@ -1,4 +1,5 @@ -# Copyright (C) 2012 Swift Navigation Inc. +# Copyright (C) 2012,2016 Swift Navigation Inc. +# Contact: Adel Mamin # # 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. @@ -32,12 +33,18 @@ cdef extern from "complexobject.h": def track_correlate_(np.ndarray[char, ndim=1, mode="c"] samples, code_freq, code_phase, carr_freq, carr_phase, np.ndarray[char, ndim=1, mode="c"] code, - sampling_freq): + sampling_freq, signal): cdef double init_code_phase = code_phase cdef double init_carr_phase = carr_phase cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L cdef unsigned int blksize - track_correlate(&samples[0], &code[0], + if signal == "L1C/A": + l1_ca_track_correlate(&samples[0], len(samples), &code[0], + &init_code_phase, code_freq / sampling_freq, + &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq, + &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize) + else: + l2c_cm_track_correlate(&samples[0], len(samples), &code[0], &init_code_phase, code_freq / sampling_freq, &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq, &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize) diff --git a/src/correlate.c b/src/correlate.c index afe1f32e..4ad75c3e 100644 --- a/src/correlate.c +++ b/src/correlate.c @@ -1,6 +1,7 @@ /* - * Copyright (C) 2013 Swift Navigation Inc. + * Copyright (C) 2013,2016 Swift Navigation Inc. * Contact: Fergus Noble + * Contact: Adel Mamin * * 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. @@ -11,6 +12,7 @@ */ #include +#include #ifdef __SSSE3__ #include @@ -22,11 +24,224 @@ * Correlators used for tracking. * \{ */ +enum correlator_type { + L1CA_CORRELATOR, + L2C_CORRELATOR +}; + +#define L1_CA_CHIPS_PER_PRN_CODE 1023 +#define L2C_CM_CHIPS_PER_PRN_CODE 10230 + +static void track_correlate(enum correlator_type correlator_type, + const s8* restrict samples, + const s8* restrict code, + double* restrict init_code_phase, double code_step, + double* restrict init_carr_phase, double carr_step, + double* restrict I_E, double* restrict Q_E, + double* restrict I_P, double* restrict Q_P, + double* restrict I_L, double* restrict Q_L, + u32 num_samples); + +/** Perform L1C/A correlation. + * + * \param samples Samples array. One byte per sample. + * \param samples_len Samples array size. + * \param code L1C/A PRN code. One byte per chip: 1023 bytes long. + * \param[in/out] init_code_phase Initial code phase [chips]. + * The function returns the + * the last unprocessed code phase here. + * \param code_step Code phase increment step [chips]. + * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * The function returns the the last unprocessed carrier + * phase here. + * \param carr_step Carrier phase increment step [radians]. + * \param[out] I_E Early replica in-phase correlation component. + * \param[out] Q_E Early replica quadrature correlation component. + * \param[out] I_P Prompt replica in-phase correlation component. + * \param[out] Q_P Prompt replica quadrature correlation component. + * \param[out] I_L Late replica in-phase correlation component. + * \param[out] Q_L Late replica quadrature correlation component. + * \param[out] num_samples The number of processed samples from \e samples array. + */ +void l1_ca_track_correlate(const s8* restrict samples, size_t samples_len, + const s8* restrict 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) +{ + *num_samples = (int)ceil((L1_CA_CHIPS_PER_PRN_CODE - *init_code_phase) / + code_step); + + if (0 == *num_samples) { + *num_samples = (int)ceil(2 * L1_CA_CHIPS_PER_PRN_CODE / code_step); + } + + if (*num_samples > samples_len) { + *num_samples = samples_len; + } + + if (0 == *num_samples) { + return; + } + + track_correlate(L1CA_CORRELATOR, samples, code, + init_code_phase, code_step, init_carr_phase, carr_step, + I_E, Q_E, I_P, Q_P, I_L, Q_L, *num_samples); +} + +/** Perform L2C CM correlation. + * + * \param samples Samples array. One byte per sample. + * \param samples_len Samples array size. + * \param code L2C CM PRN code. One byte per chip: 10230 bytes long. + * \param[in/out] init_code_phase Initial code phase [chips]. + * The function returns the + * the last unprocessed code phase here. + * \param code_step Code phase increment step [chips]. + * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * The function returns the the last unprocessed carrier + * phase here. + * \param carr_step Carrier phase increment step [radians]. + * \param[out] I_E Early replica in-phase correlation component. + * \param[out] Q_E Early replica quadrature correlation component. + * \param[out] I_P Prompt replica in-phase correlation component. + * \param[out] Q_P Prompt replica quadrature correlation component. + * \param[out] I_L Late replica in-phase correlation component. + * \param[out] Q_L Late replica quadrature correlation component. + * \param[out] num_samples The number of processed samples from \e samples array. + */ +void l2c_cm_track_correlate(const s8* samples, size_t samples_len, + const 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) +{ + *num_samples = (int)ceil((2 * L2C_CM_CHIPS_PER_PRN_CODE - *init_code_phase) / + code_step); + + if (0 == *num_samples) { + *num_samples = (int)ceil(2 * L2C_CM_CHIPS_PER_PRN_CODE / code_step); + } + + if (*num_samples > samples_len) { + *num_samples = samples_len; + } + + if (0 == *num_samples) { + return; + } + + track_correlate(L2C_CORRELATOR, samples, code, + init_code_phase, code_step, init_carr_phase, carr_step, + I_E, Q_E, I_P, Q_P, I_L, Q_L, *num_samples); +} + +/** Produce L1 C/A chip for the given code phase + * + * \param code L1 C/A PRN code array. One byte per sample: 1023 bytes long. + * \param code_phase Code phase of the chip to return. + * \return Code chip. + */ +static inline s8 l1_ca_get_chip(const s8 *code, double code_phase) +{ + int i; + if (code_phase < 0) { + i = (int)(code_phase + L1_CA_CHIPS_PER_PRN_CODE); + } else if (code_phase >= L1_CA_CHIPS_PER_PRN_CODE) { + i = (int)(code_phase - L1_CA_CHIPS_PER_PRN_CODE); + } else { + i = (int)code_phase; + } + return code[i]; +} + +/** Produce a new L1 C/A code phase given current code phase and the step. + * + * \param code_phase Current code phase [chips]. + * \param code_step Code phase update step [chips]. + * \return New code phase. + */ +static inline double l1_ca_get_code_phase(double code_phase, double code_step) +{ + code_phase += code_step; + if (code_phase >= L1_CA_CHIPS_PER_PRN_CODE) { + code_phase -= L1_CA_CHIPS_PER_PRN_CODE; + } + return code_phase; +} + +/** Produce L2C CM chip for the given code phase + * + * \param code L2C CM PRN code array. One byte per sample: 10230 bytes long. + * \param code_phase Code phase of the chip to return. + * \return Code chip. + */ +static inline s8 l2c_cm_get_chip(const s8 *code, double code_phase) +{ + if (code_phase < 0) { + code_phase += 2 * L2C_CM_CHIPS_PER_PRN_CODE; + } else if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) { + code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE; + } + if ((int)code_phase & 1) { + return 0; // hit CL code, which is neglected by design + } + + return -code[(int)code_phase / 2]; // otherwise return CM code +} + +/** Produce a new L2C CM code phase given current code phase and the step. + * + * \param code_phase Current code phase [chips]. + * \param code_step Code phase update step [chips]. + * \return New code phase. + */ +static inline double l2c_cm_get_code_phase(double code_phase, double code_step) +{ + code_phase += code_step; + if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) { + code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE; + } + return code_phase; +} + #ifndef __SSSE3__ -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) +/** Perform correlation. + * + * \param correlator_type Correlator type. L1 C/A or L2C CM are supported. + * \param samples Samples array. One byte per sample. + * \param code PRN code. One byte per chip. + * \param[in/out] init_code_phase Initial code phase [chips]. + * The function returns the last unprocessed code + * phase here. + * \param code_step Code phase increment step [chips]. + * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * The function returns the the last unprocessed carrier + * phase here. + * \param carr_step Carrier phase increment step [radians]. + * \param[out] I_E Early replica in-phase correlation component. + * \param[out] Q_E Early replica quadrature correlation component. + * \param[out] I_P Prompt replica in-phase correlation component. + * \param[out] Q_P Prompt replica quadrature correlation component. + * \param[out] I_L Late replica in-phase correlation component. + * \param[out] Q_L Late replica quadrature correlation component. + * \param num_samples The number of samples to correlate from \e samples + * array. + */ +static void track_correlate(enum correlator_type correlator_type, + const s8* restrict samples, + const s8* restrict code, + double* restrict init_code_phase, double code_step, + double* restrict init_carr_phase, double carr_step, + double* restrict I_E, double* restrict Q_E, + double* restrict I_P, double* restrict Q_P, + double* restrict I_L, double* restrict Q_L, + u32 num_samples) { double code_phase = *init_code_phase; double carr_phase = *init_carr_phase; @@ -38,25 +253,36 @@ void track_correlate(s8* samples, s8* code, *I_E = *Q_E = *I_P = *Q_P = *I_L = *Q_L = 0; - double code_E, code_P, code_L; + s8 code_E, code_P, code_L; double baseband_Q, baseband_I; - *num_samples = (int)ceil((1023.0 - code_phase) / code_step); + for (u32 i=0; i +#include +#include +#include +#include + +#define L1CA_CHIPS_PER_PRN_CODE 1023 +#define L2C_CM_CHIPS_PER_PRN_CODE 10230 + +enum signal_type { + L1CA_SIGNAL, + L2C_SIGNAL +}; + +//#define DUMP_RESULTS + +#ifdef DUMP_RESULTS +#include + +static clock_t start = 0; +static clock_t stop = 0; + +static void debug_start_timing(void) +{ + start = clock(); +} + +static void debug_stop_timing(void) +{ + stop = clock(); +} + +static void debug_dump_results(u32 num_samples, + double I_E, double Q_E, + double I_P, double Q_P, + double I_L, double Q_L) +{ + printf("==============================================\n"); + printf("used %f cycles\n", (double)stop - start); + + printf("num_samples = %d\n", num_samples); + + printf("\n"); + + printf("I_E / I_P = %f\n", (0 == I_P) ? 0 : I_E / I_P); + printf("I_L / I_P = %f\n", (0 == I_P) ? 0 : I_L / I_P); + + printf("\n"); + + printf("I_E = %f\n", I_E); + printf("Q_E = %f\n", Q_E); + printf("Q_E / I_E = %f\n", (0 == I_E) ? 0 : Q_E / I_E); + + printf("\n"); + + printf("I_P = %f\n", I_P); + printf("Q_P = %f\n", Q_P); + printf("Q_P / I_P = %f\n", (0 == I_P) ? 0 : Q_P / I_P); + + printf("\n"); + + printf("I_L = %f\n", I_L); + printf("Q_L = %f\n", Q_L); + printf("Q_L / I_L = %f\n", (0 == I_L) ? 0 : Q_L / I_L); + + printf("\n"); +} +#else +#define debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L) +#define debug_start_timing() +#define debug_stop_timing() +#endif + +/* PRN 1. One bit per sample. */ +static const u8 gps_l1ca_code[] = { + 0xC8, 0x39, 0x49, 0xE5, 0x13, 0xEA, 0xD1, 0x15, 0x59, 0x1E, 0x9F, + 0xB7, 0x37, 0xCA, 0xA1, 0x00, 0xEA, 0x44, 0xDE, 0x0F, 0x5C, 0xCF, + 0x60, 0x2F, 0x3E, 0xA6, 0x2D, 0xC6, 0xF5, 0x15, 0x82, 0x01, 0x03, + 0x1D, 0x81, 0xC6, 0xFF, 0xA7, 0x4B, 0x61, 0x56, 0x27, 0x2D, 0xD8, + 0xEE, 0xF0, 0xD8, 0x64, 0x90, 0x6D, 0x2D, 0xE2, 0xE0, 0x52, 0x7E, + 0x0A, 0xB9, 0xF5, 0xF3, 0x31, 0xC6, 0xD5, 0x6C, 0x6E, 0xE0, 0x02, + 0xCD, 0x9D, 0xA0, 0xAB, 0xAE, 0x94, 0x73, 0x89, 0x45, 0x2D, 0x0A, + 0xDA, 0xD8, 0xE7, 0xB2, 0x1F, 0x96, 0x88, 0x7D, 0x5C, 0xC9, 0x25, + 0xFF, 0x87, 0xDE, 0x37, 0x2C, 0x39, 0x50, 0xA5, 0x7E, 0x3D, 0xA7, + 0x67, 0xEF, 0xA3, 0x1F, 0x01, 0x28, 0xB4, 0x44, 0xD8, 0x1D, 0xA3, + 0x44, 0x8E, 0x2C, 0xC9, 0xE6, 0xFC, 0xCA, 0x69, 0xAF, 0x36, 0xA7, + 0x78, 0xD4, 0x42, 0x24, 0xE1, 0xCA, 0x20 +}; + +static const u8 gps_l2cm_code[] = { + 0x2B, 0xDE, 0x1E, 0xBA, 0x29, 0x87, 0xCB, 0x12, 0xA8, 0xFA, 0xF0, + 0xFA, 0xBA, 0xD8, 0x1E, 0xCC, 0x07, 0xB3, 0xF1, 0x84, 0xCB, 0xDA, + 0x5D, 0xE0, 0x48, 0x62, 0x87, 0xCF, 0xC9, 0x69, 0xE0, 0x39, 0x5C, + 0x77, 0x26, 0xB1, 0x4B, 0x36, 0xF6, 0xA9, 0xBC, 0x16, 0x85, 0x16, + 0x39, 0x3A, 0x5D, 0x7B, 0xD6, 0x28, 0xBD, 0x8F, 0xDF, 0x9A, 0xE9, + 0x10, 0x2E, 0xAD, 0xD0, 0xCA, 0x1C, 0xE4, 0xBE, 0x05, 0x97, 0x38, + 0xB4, 0x71, 0xC0, 0xB2, 0xEC, 0x7F, 0x4B, 0x34, 0xA4, 0xA3, 0xF6, + 0x7A, 0x3C, 0x5B, 0x3A, 0xAA, 0x87, 0x98, 0x53, 0xF0, 0xB3, 0x9A, + 0xC5, 0xE6, 0xD9, 0x7F, 0x26, 0x1D, 0x71, 0xB7, 0x75, 0xA8, 0x0C, + 0x77, 0x78, 0xCF, 0x3C, 0xC6, 0xF4, 0x4E, 0x20, 0x62, 0x8B, 0x2B, + 0x2B, 0x4E, 0xBE, 0xF3, 0x67, 0xD1, 0x65, 0x49, 0x11, 0x83, 0xB9, + 0x21, 0xBA, 0x38, 0xB5, 0x21, 0x3D, 0x5E, 0xC1, 0x7D, 0xF8, 0x43, + 0x58, 0x37, 0x45, 0xE8, 0x27, 0xAD, 0x99, 0x46, 0xC6, 0x96, 0xB8, + 0xE6, 0x3B, 0x51, 0xC2, 0x36, 0xD8, 0xA3, 0x9D, 0x39, 0x0A, 0x65, + 0x1E, 0xEA, 0xEC, 0x8E, 0x0A, 0xF3, 0xC1, 0x0A, 0xA7, 0xEB, 0x80, + 0x32, 0x82, 0x3D, 0x5B, 0x35, 0xDA, 0xCA, 0xFA, 0x94, 0x83, 0x9A, + 0x96, 0x6A, 0xB0, 0xD8, 0xD7, 0x69, 0x7C, 0x73, 0xDF, 0xC2, 0x54, + 0x25, 0x42, 0x4B, 0xA2, 0xED, 0x1B, 0x5E, 0xC7, 0xB4, 0x76, 0xA7, + 0x1D, 0x9B, 0x4D, 0x03, 0xCB, 0xDA, 0x81, 0xAD, 0xF3, 0x60, 0x76, + 0x23, 0xC6, 0x24, 0xC1, 0x90, 0xB3, 0x3C, 0xAA, 0x56, 0xED, 0xD3, + 0x25, 0x17, 0xD6, 0x3E, 0x6F, 0xA0, 0x7B, 0x52, 0x9E, 0xAE, 0xE9, + 0xD3, 0x26, 0xE3, 0x64, 0xE8, 0x9E, 0xFE, 0xD1, 0xF6, 0x52, 0xBE, + 0x5A, 0xB2, 0x60, 0xDC, 0xEF, 0x30, 0x16, 0x1E, 0x89, 0xD6, 0xBD, + 0xB0, 0xE6, 0x68, 0xC0, 0x5F, 0x94, 0x71, 0xBC, 0x0F, 0xD8, 0xB6, + 0x8C, 0x7F, 0x93, 0x90, 0x96, 0x97, 0xA2, 0x16, 0xA6, 0x2E, 0x86, + 0xD5, 0x4D, 0xF3, 0x65, 0xDC, 0x7E, 0x3E, 0x7E, 0xC3, 0xA8, 0x9C, + 0x11, 0x9C, 0x51, 0x0D, 0x8A, 0xE8, 0x6C, 0x35, 0xF3, 0x53, 0x91, + 0x30, 0x35, 0x65, 0x51, 0x99, 0x7D, 0xFF, 0x95, 0x31, 0x34, 0xCC, + 0x4B, 0x96, 0x54, 0x1A, 0xA7, 0x4F, 0x81, 0x1D, 0xF7, 0x1B, 0x2C, + 0xAB, 0xFD, 0xBB, 0x9C, 0xC3, 0x6D, 0x3B, 0xE1, 0x18, 0x55, 0x4E, + 0x62, 0x6D, 0xE9, 0xF2, 0xB1, 0x34, 0x3E, 0x65, 0x44, 0x37, 0x20, + 0x35, 0x51, 0xB9, 0x86, 0x40, 0x28, 0x77, 0x21, 0x6F, 0x4E, 0xC1, + 0x65, 0x12, 0xF5, 0xFA, 0xA0, 0x13, 0xFF, 0xFA, 0x2D, 0x59, 0xDE, + 0x4C, 0xC5, 0xD0, 0x02, 0xF5, 0x5A, 0x45, 0x98, 0x51, 0x15, 0x80, + 0x3D, 0xCF, 0xFB, 0x3E, 0x04, 0x48, 0x02, 0x1C, 0x0F, 0x74, 0xB4, + 0xAE, 0x8D, 0x79, 0xE1, 0x52, 0xE3, 0xC0, 0xC7, 0xC1, 0x70, 0x05, + 0x25, 0x95, 0xE3, 0xB6, 0xB5, 0x33, 0x98, 0x50, 0xD0, 0x49, 0x6C, + 0x47, 0x33, 0xB4, 0xB9, 0x73, 0x12, 0x79, 0xDA, 0x8A, 0x4F, 0x9A, + 0x8A, 0x44, 0xB4, 0xCC, 0x4C, 0x0D, 0x3C, 0x2F, 0xC9, 0x67, 0x04, + 0x84, 0x8E, 0x41, 0x94, 0x27, 0x26, 0xF7, 0x3B, 0xEC, 0x95, 0x25, + 0x26, 0x3C, 0x8D, 0x5A, 0x33, 0x0C, 0x3F, 0x65, 0x51, 0x1C, 0xB7, + 0x8F, 0x22, 0x14, 0xB3, 0xFA, 0x10, 0x6C, 0x29, 0x1E, 0xF5, 0x06, + 0xF7, 0x4D, 0x68, 0x22, 0xBD, 0xC3, 0x2C, 0x05, 0x05, 0x62, 0xD4, + 0x5B, 0x32, 0xF7, 0x19, 0xBA, 0x17, 0x5A, 0xB6, 0x41, 0xF7, 0xF0, + 0xAE, 0x5A, 0xEB, 0x9F, 0x98, 0xF0, 0x84, 0x6D, 0xD9, 0x78, 0x17, + 0x4D, 0x57, 0x17, 0x89, 0xF0, 0xEF, 0xD2, 0x66, 0x4E, 0x22, 0xD1, + 0xDB, 0x49, 0x28, 0x9E, 0xC5, 0x4A, 0xC8, 0x53, 0xD3, 0x03, 0x5E, + 0x23, 0xB6, 0x57, 0x1F, 0xB5, 0x98, 0xA9, 0x1E, 0x1C, 0x23, 0xB9, + 0xAB, 0x76, 0xCB, 0x79, 0xB2, 0x4B, 0x05, 0x6B, 0x2E, 0xD8, 0x31, + 0x83, 0xDF, 0x15, 0x8A, 0xFF, 0x03, 0x12, 0xE6, 0x3E, 0x82, 0x67, + 0x07, 0x99, 0xFF, 0xB0, 0xE0, 0x13, 0xAA, 0xDB, 0x5A, 0x42, 0xB2, + 0xD8, 0x71, 0x73, 0x20, 0x4A, 0x32, 0x37, 0x52, 0xC4, 0x29, 0x52, + 0x69, 0x2E, 0xCC, 0xA6, 0xD0, 0x06, 0x05, 0xAA, 0xBD, 0xD5, 0x11, + 0x16, 0x1F, 0xF2, 0x86, 0x27, 0x82, 0x7B, 0x2E, 0x89, 0x85, 0x16, + 0xFC, 0x67, 0x47, 0xFA, 0x85, 0x81, 0x6D, 0xE2, 0xBB, 0x97, 0xAE, + 0xB9, 0x68, 0x78, 0x34, 0x1E, 0x00, 0x3A, 0x66, 0x54, 0x21, 0x0E, + 0x50, 0xC3, 0x77, 0xEA, 0x7C, 0xD1, 0x81, 0xCE, 0xDD, 0x6E, 0x80, + 0xD9, 0x97, 0xC4, 0xE8, 0x94, 0x4C, 0xB0, 0x0B, 0xBD, 0x30, 0x0B, + 0x8A, 0x64, 0xE6, 0x4E, 0xD3, 0x93, 0x57, 0x57, 0xDA, 0xD9, 0xCF, + 0x35, 0x37, 0x39, 0xAD, 0x7E, 0xC8, 0x94, 0x18, 0xCC, 0x17, 0x16, + 0xA0, 0x60, 0x97, 0x36, 0xF4, 0x40, 0x49, 0xAB, 0x03, 0x66, 0x61, + 0x01, 0x00, 0x2A, 0x00, 0x1C, 0xF7, 0xB3, 0x07, 0x52, 0x70, 0x4E, + 0xC0, 0x31, 0x45, 0xF7, 0x41, 0xD7, 0xE7, 0xDD, 0x93, 0x13, 0x1E, + 0x17, 0xC9, 0x20, 0x21, 0xC5, 0x3D, 0xB0, 0x90, 0x67, 0x95, 0x6B, + 0xBC, 0xB4, 0xDA, 0x04, 0xBC, 0x61, 0xB3, 0xD1, 0x8A, 0x5D, 0x49, + 0xD6, 0xD8, 0x6A, 0xA3, 0x37, 0xC4, 0xFF, 0x00, 0xF6, 0xC3, 0x4A, + 0x0A, 0xB4, 0xC0, 0xB9, 0xDD, 0x04, 0xA5, 0x05, 0x7C, 0xC7, 0xE2, + 0xBB, 0x99, 0x86, 0x65, 0x12, 0x88, 0x5D, 0x7C, 0x6E, 0xCF, 0x40, + 0xA9, 0xCC, 0x8F, 0xF4, 0x4A, 0xEA, 0x0D, 0x1A, 0xAD, 0xF2, 0x98, + 0x7A, 0x3F, 0xF5, 0xB1, 0x37, 0x68, 0x1F, 0xE1, 0x89, 0xCB, 0x81, + 0xC5, 0x49, 0xEE, 0x56, 0xBC, 0x5C, 0xC4, 0xAA, 0x8F, 0x5D, 0x6F, + 0x33, 0x83, 0x07, 0xE3, 0x71, 0xE0, 0xC8, 0xC6, 0xA7, 0xDC, 0x6A, + 0x2F, 0xE2, 0x5B, 0x19, 0x33, 0xD2, 0x10, 0x3C, 0xB9, 0x56, 0xE5, + 0xDE, 0x27, 0x69, 0x46, 0x16, 0x12, 0x46, 0xD7, 0xA6, 0xBC, 0x0F, + 0x3B, 0xF7, 0x99, 0x16, 0x7C, 0x98, 0x29, 0xC1, 0x43, 0x50, 0x9C, + 0x4C, 0x64, 0x05, 0x27, 0x6E, 0xBD, 0xB1, 0x6A, 0x96, 0xDD, 0x3D, + 0xDB, 0xB3, 0x6B, 0x1D, 0x01, 0xC5, 0x29, 0x15, 0xAC, 0xB7, 0xCF, + 0xB3, 0xE8, 0x73, 0x3A, 0x13, 0xBD, 0x98, 0xAD, 0x20, 0x96, 0x4E, + 0x34, 0xB6, 0xBF, 0x52, 0xEB, 0xA9, 0x3C, 0x88, 0x53, 0x03, 0x3A, + 0x87, 0x59, 0x3A, 0xE2, 0x6D, 0xA8, 0xF4, 0xD1, 0x1A, 0x72, 0x6F, + 0xFF, 0xF3, 0xBA, 0x1F, 0x58, 0xCC, 0xA5, 0x40, 0xB8, 0x1F, 0x18, + 0xD6, 0xF5, 0x66, 0xF8, 0x75, 0xBA, 0x9C, 0xD0, 0xD0, 0x5A, 0xED, + 0x46, 0xCF, 0xFC, 0xF5, 0xF0, 0x21, 0xB8, 0xB6, 0xF2, 0xD0, 0x53, + 0x16, 0x60, 0x71, 0x25, 0xB8, 0x12, 0x5D, 0xD6, 0x6A, 0xD5, 0x11, + 0x00, 0xA5, 0x17, 0xF4, 0x20, 0xAD, 0xF0, 0x7A, 0xE6, 0x47, 0x7F, + 0x73, 0x91, 0x2A, 0xD1, 0xEA, 0x90, 0x51, 0x1F, 0xB1, 0x4E, 0xFE, + 0x58, 0x04, 0x78, 0x6E, 0xD3, 0x15, 0x69, 0x09, 0x2F, 0x96, 0x6F, + 0x92, 0x97, 0x49, 0x18, 0x1A, 0x4B, 0xEF, 0xDB, 0xD1, 0x77, 0xA6, + 0xFF, 0x31, 0xA7, 0x5D, 0x56, 0xA5, 0xCF, 0x1E, 0x8A, 0x0A, 0xCE, + 0x4E, 0x75, 0x61, 0x4A, 0xD1, 0x39, 0x13, 0xEC, 0x6A, 0xEB, 0x41, + 0xB2, 0x1D, 0x8E, 0xA1, 0xD2, 0x56, 0x2E, 0xF9, 0x0F, 0x9C, 0x5C, + 0xB0, 0x35, 0x05, 0xC3, 0x0C, 0x1D, 0x0E, 0x31, 0x0B, 0xBA, 0x71, + 0x6A, 0x2C, 0x68, 0xD7, 0xB6, 0x32, 0x6F, 0x3D, 0x2F, 0x93, 0x71, + 0xBD, 0xF9, 0x2E, 0xA8, 0x13, 0xDF, 0xAA, 0xFA, 0xB1, 0x0B, 0x1D, + 0x1E, 0x69, 0x76, 0xEB, 0x91, 0xD1, 0x2F, 0x61, 0xE5, 0x7B, 0x9A, + 0x1E, 0xC4, 0xC8, 0x91, 0x94, 0xC3, 0xAA, 0xE9, 0xA2, 0x64, 0x78, + 0x83, 0x2B, 0x65, 0x0C, 0x03, 0x2F, 0x7B, 0x1F, 0x05, 0x6F, 0x98, + 0x67, 0x1F, 0x03, 0x78, 0x92, 0xDB, 0x88, 0x1D, 0x04, 0x83, 0xF6, + 0xD3, 0x05, 0x9D, 0x46, 0x0F, 0x95, 0x54, 0x40, 0x3C, 0xCC, 0xEC, + 0x8B, 0x5A, 0xD8, 0xA8, 0x9C, 0x36, 0xD0, 0x18, 0xB8, 0xA6, 0xEC, + 0xC5, 0xF1, 0x1D, 0xE2, 0xB8, 0x35, 0xE3, 0x3E, 0x0B, 0x92, 0x39, + 0x27, 0x4B, 0x6F, 0xDD, 0x77, 0x5D, 0x07, 0xB0, 0xB0, 0x5A, 0x87, + 0x06, 0x9D, 0x43, 0x3E, 0x9A, 0x87, 0x88, 0xB8, 0x39, 0xC4, 0xAC, + 0x71, 0x46, 0x2A, 0xF7, 0x32, 0xFB, 0x8A, 0x14, 0xB1, 0xFE, 0xAB, + 0x3D, 0x9A, 0xAF, 0x82, 0x37, 0x43, 0xBE, 0xEA, 0x3D, 0x65, 0xBE, + 0x31, 0xA7, 0x5C, 0x48, 0xA9, 0xDF, 0x32, 0xB2, 0x63, 0x92, 0x49, + 0xE6, 0xED, 0xBE, 0xE6, 0x04, 0xD4, 0x2E, 0x7D, 0xB0, 0xB3, 0x88, + 0xFB, 0x01, 0x52, 0x64, 0x93, 0xA1, 0x95, 0xA9, 0xFA, 0x4D, 0x07, + 0xF8, 0x5A, 0x24, 0x6A, 0x6A, 0x88, 0x36, 0x3B, 0x20, 0xC1, 0x24, + 0x59, 0x46, 0x3C, 0xDD, 0x57, 0xD6, 0xAE, 0xA8, 0x7C, 0x5C, 0xCA, + 0xD6, 0x42, 0xB0, 0xFB, 0x73, 0x90, 0xD6, 0xCC, 0xA9, 0x88, 0x16, + 0xDB, 0x94, 0xE0, 0xE7, 0x68, 0x9E, 0xE3, 0xC3, 0x9A, 0x69, 0xA1, + 0x0A, 0xDB, 0xAA, 0x86, 0x42, 0xE9, 0x0D, 0xFE, 0xC6, 0xD5, 0x24, + 0xDB, 0x8D, 0xBA, 0xA6, 0x48, 0x16, 0x4E, 0xF4, 0x4C, 0xD5, 0xC6, + 0x8E, 0x58, 0xBB, 0xC4, 0x41, 0x73, 0xDC, 0x4A, 0xAA, 0xD8, 0x2E, + 0x12, 0x62, 0x83, 0x2A, 0x1A, 0xD1, 0x52, 0xAD, 0x4D, 0x50, 0xCE, + 0xF1, 0x06, 0x28 +}; + +struct signal { + s8* samples; + size_t size; +}; + +struct signal generate_signal(enum signal_type signal_type, + double if_freq, double code_freq, + double carr_doppler_freq, + double carr_to_code, double sampling_freq, + s8* prn_code, u32 ms_to_generate) +{ + u32 i; + double delt; + double code_phase = 0; + double carr_phase = 0; + u32 samples_num; + double carr_freq; + double carr_phasestep; + double code_phasestep; + s8* samples; + struct signal signal = {NULL, 0}; + + samples_num = (u32)(sampling_freq * 1e-3 * ms_to_generate); + samples = malloc(samples_num); + if (NULL == samples) { + return signal; + } + delt = 1. / sampling_freq; + carr_freq = if_freq + carr_doppler_freq; + code_freq += carr_doppler_freq * carr_to_code; + carr_phasestep = carr_freq * delt; + code_phasestep = code_freq * delt; + + for (i = 0; i < samples_num; i++) { + s8 code = 0; + double ip; + switch (signal_type) { + case L1CA_SIGNAL: + code = prn_code[(int)code_phase]; + break; + case L2C_SIGNAL: + if ((int)code_phase & 1) { + code = 0; // hit CL + } else { + code = prn_code[(int)code_phase / 2]; + } + break; + } + + static bool header = 1; + static FILE *f; + if (header) { + f = fopen("./gen.csv", "w"); + fprintf(f, "code_phase,prn\n"); + header = 0; + } + fprintf(f, "%f,%d\n", code_phase, code); + + ip = code * cos(carr_phase * 2 * M_PI); + if (ip > 0) { + samples[i] = 1; + } else if (ip < 0) { + samples[i] = -1; + } + + carr_phase += carr_phasestep; + code_phase += code_phasestep; + + switch (signal_type) { + case L1CA_SIGNAL: + if (code_phase >= L1CA_CHIPS_PER_PRN_CODE) { + code_phase -= L1CA_CHIPS_PER_PRN_CODE; + } + break; + case L2C_SIGNAL: + if (code_phase >= 2 * L2C_CM_CHIPS_PER_PRN_CODE) { + code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE; + } + break; + } + } + signal.samples = samples; + signal.size = samples_num; + + return signal; +} + +static s8* get_prn_code(const u8 *code, size_t code_size, + size_t chips_per_prn) +{ + s8* prn_code; + u32 i = 0; + u32 j; + u32 k; + u8 packed; + + prn_code = malloc(chips_per_prn); + if (NULL == prn_code) { + return NULL; + } + j = 0; + do { + packed = code[j++]; + for (k = 0; k < 8; k++) { + prn_code[i++] = (packed & 0x80) ? 1 : -1; + packed <<= 1; + if (i >= chips_per_prn) { + break; + } + } + } while ((i < chips_per_prn) && (j < code_size)); + return prn_code; +} + +#define SAMPLING_FREQ_HZ 25e6 +#define L1CA_CHIPPING_RATE_HZ 1.023e6 +#define L2C_CM_CHIPPING_RATE_HZ 1.023e6 +#define IF_FREQUENCY_HZ 2e6 +#define CARRIER_DOPPLER_FREQ_HZ 100 + +START_TEST(test_l1ca_correlator) +{ + struct signal signal; + s8* code; + double init_code_phase = 0; + double init_carr_phase = 0; + double I_E; + double Q_E; + double I_P; + double Q_P; + double I_L; + double Q_L; + u32 num_samples; + + code = get_prn_code(gps_l1ca_code, sizeof(gps_l1ca_code), + L1CA_CHIPS_PER_PRN_CODE); + fail_if(NULL == code, "Could not allocate PRN code data"); + + signal = generate_signal( L1CA_SIGNAL, /* signal type */ + IF_FREQUENCY_HZ, /* intermediate frequency */ + L1CA_CHIPPING_RATE_HZ, /* code frequency */ + CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */ + 1. / 1540, /* carrier to code scaling factor */ + SAMPLING_FREQ_HZ, /* sampling frequency */ + code, /* PRN code data */ + 1); /* milliseconds to generate */ + + fail_if(NULL == signal.samples, "Could not generate signal data"); + + debug_start_timing(); + + l1_ca_track_correlate(signal.samples, signal.size, + code, + &init_code_phase, + L1CA_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ, + &init_carr_phase, + (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ, + &I_E, &Q_E, + &I_P, &Q_P, + &I_L, &Q_L, + &num_samples); + + debug_stop_timing(); + + debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L); + + u32 expected_samples = SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */ + fail_if(num_samples != expected_samples); + + fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.5)); + fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.5)); + + fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006)); + fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005)); + fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006)); + + free(code); + free(signal.samples); +} +END_TEST + +START_TEST(test_l2c_cm_correlator) +{ + struct signal signal; + s8* code; + double init_code_phase = 0; + double init_carr_phase = 0; + double I_E; + double Q_E; + double I_P; + double Q_P; + double I_L; + double Q_L; + u32 num_samples; + + code = get_prn_code(gps_l2cm_code, sizeof(gps_l2cm_code), + L2C_CM_CHIPS_PER_PRN_CODE); + fail_if(NULL == code, "Could not allocate L2C CM PRN code data"); + + signal = generate_signal( L2C_SIGNAL, /* signal type */ + IF_FREQUENCY_HZ, /* intermediate frequency */ + L2C_CM_CHIPPING_RATE_HZ, /* code frequency */ + CARRIER_DOPPLER_FREQ_HZ,/* carr_doppler frequency */ + 1. / 1200, /* carrier to code scaling factor */ + SAMPLING_FREQ_HZ, /* sampling frequency */ + code, /* PRN code data */ + 20); /* milliseconds to generate */ + + fail_if(NULL == signal.samples, "Could not generate signal data"); + + debug_start_timing(); + + l2c_cm_track_correlate(signal.samples, signal.size, + code, + &init_code_phase, + L2C_CM_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ, + &init_carr_phase, + (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ, + &I_E, &Q_E, + &I_P, &Q_P, + &I_L, &Q_L, + &num_samples); + + debug_stop_timing(); + + debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L); + + u32 expected_samples = 20 * SAMPLING_FREQ_HZ / 1000; /* samples per PRN code */ + fail_if(num_samples != expected_samples); + + fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.6)); + fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.6)); + + fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006)); + fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005)); + fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006)); + + free(code); + free(signal.samples); +} +END_TEST + +Suite* correlator_suite(void) +{ + Suite *s = suite_create("Correlator"); + TCase *tc_core = tcase_create("Core"); + + tcase_add_test(tc_core, test_l1ca_correlator); + tcase_add_test(tc_core, test_l2c_cm_correlator); + suite_add_tcase(s, tc_core); + + return s; +} diff --git a/tests/check_main.c b/tests/check_main.c index d52d68f7..0f937e23 100644 --- a/tests/check_main.c +++ b/tests/check_main.c @@ -33,6 +33,7 @@ int main(void) srunner_add_suite(sr, signal_test_suite()); srunner_add_suite(sr, track_test_suite()); srunner_add_suite(sr, cnav_test_suite()); + srunner_add_suite(sr, correlator_suite()); srunner_set_fork_status(sr, CK_NOFORK); srunner_run_all(sr, CK_NORMAL); diff --git a/tests/check_suites.h b/tests/check_suites.h index 71c6ec07..c572242a 100644 --- a/tests/check_suites.h +++ b/tests/check_suites.h @@ -23,5 +23,6 @@ Suite* ionosphere_suite(void); Suite* signal_test_suite(void); Suite* track_test_suite(void); Suite* cnav_test_suite(void); +Suite* correlator_suite(void); #endif /* CHECK_SUITES_H */ From f1321b063ce25e65f34093562229ea88aece748a Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 8 Feb 2016 10:19:13 +0200 Subject: [PATCH 02/18] Enable debug output in correlator unit test --- tests/check_correlator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/check_correlator.c b/tests/check_correlator.c index 04e080d4..86c1e914 100644 --- a/tests/check_correlator.c +++ b/tests/check_correlator.c @@ -12,7 +12,7 @@ enum signal_type { L2C_SIGNAL }; -//#define DUMP_RESULTS +#define DUMP_RESULTS #ifdef DUMP_RESULTS #include From 14997a16ed1753c09ed17eb3d1477b7c03a8c677 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 8 Feb 2016 11:15:32 +0200 Subject: [PATCH 03/18] Disable SSSE3 for testing --- src/correlate.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/correlate.c b/src/correlate.c index 4ad75c3e..2f383294 100644 --- a/src/correlate.c +++ b/src/correlate.c @@ -14,6 +14,10 @@ #include #include +#ifdef __SSSE3__ +#undef __SSSE3__ +#endif + #ifdef __SSSE3__ #include #endif From fc126dfab7612ff02ed703c2ba7e0d9c8db9bfd3 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 8 Feb 2016 14:43:20 +0200 Subject: [PATCH 04/18] Fix bug in correlator Check unit test code generator --- src/correlate.c | 4 ---- tests/check_correlator.c | 2 ++ 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/correlate.c b/src/correlate.c index 2f383294..4ad75c3e 100644 --- a/src/correlate.c +++ b/src/correlate.c @@ -14,10 +14,6 @@ #include #include -#ifdef __SSSE3__ -#undef __SSSE3__ -#endif - #ifdef __SSSE3__ #include #endif diff --git a/tests/check_correlator.c b/tests/check_correlator.c index 86c1e914..0f027c55 100644 --- a/tests/check_correlator.c +++ b/tests/check_correlator.c @@ -270,6 +270,8 @@ struct signal generate_signal(enum signal_type signal_type, samples[i] = 1; } else if (ip < 0) { samples[i] = -1; + } else { + samples[i] = 0; } carr_phase += carr_phasestep; From f0cb62da52a329cafbbfb163622309b759460dea Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 8 Feb 2016 14:59:53 +0200 Subject: [PATCH 05/18] Disable debug output --- tests/check_correlator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/check_correlator.c b/tests/check_correlator.c index 0f027c55..e8dcd989 100644 --- a/tests/check_correlator.c +++ b/tests/check_correlator.c @@ -12,7 +12,7 @@ enum signal_type { L2C_SIGNAL }; -#define DUMP_RESULTS +//#define DUMP_RESULTS #ifdef DUMP_RESULTS #include From 8ff2a936a10c977f375b54e218e80308f3a006fc Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Thu, 11 Feb 2016 14:58:33 +0200 Subject: [PATCH 06/18] Add L2C track support --- python/swiftnav/correlate.pyx | 10 +++++----- python/swiftnav/track.pyx | 1 - 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/python/swiftnav/correlate.pyx b/python/swiftnav/correlate.pyx index 8de52071..eafa4533 100644 --- a/python/swiftnav/correlate.pyx +++ b/python/swiftnav/correlate.pyx @@ -30,15 +30,15 @@ cdef extern from "complexobject.h": ctypedef class __builtin__.complex [object PyComplexObject]: cdef Py_complex cval -def track_correlate_(np.ndarray[char, ndim=1, mode="c"] samples, - code_freq, code_phase, carr_freq, carr_phase, - np.ndarray[char, ndim=1, mode="c"] code, - sampling_freq, signal): +def track_correlate(np.ndarray[char, ndim=1, mode="c"] samples, + code_freq, code_phase, carr_freq, carr_phase, + np.ndarray[char, ndim=1, mode="c"] code, + sampling_freq, signal): cdef double init_code_phase = code_phase cdef double init_carr_phase = carr_phase cdef double I_E, Q_E, I_P, Q_P, I_L, Q_L cdef unsigned int blksize - if signal == "L1C/A": + if signal == "l1ca": l1_ca_track_correlate(&samples[0], len(samples), &code[0], &init_code_phase, code_freq / sampling_freq, &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq, diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index c6636bdc..8e6acd31 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -250,7 +250,6 @@ cdef class AidedTrackingLoop: kwargs['carr_k'], kwargs['carr_freq_b1']) - def retune(self, code_params, carr_params, loop_freq, carr_freq_igain, carr_to_code): """ Retune the tracking loop. From 13bed1fea0c580fd9cbc2e31967309374f3a54b4 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Thu, 18 Feb 2016 11:30:52 +0200 Subject: [PATCH 07/18] Fix track.pyx for lock detectors --- python/swiftnav/track.pyx | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index 8e6acd31..282fb979 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -366,12 +366,11 @@ cdef class LockDetector: """ def __cinit__(self, **kwargs): - self._thisptr = kwargs lock_detect_init(&self._thisptr, - self._thisptr.k1, - self._thisptr.k2, - self._thisptr.thislp, - self._thisptr.lo) + kwargs['k1'], + kwargs['k2'], + kwargs['lp'], + kwargs['lo']) def reinit(self, k1, k2, lp, lo): lock_detect_reinit(&self._thisptr, k1, k2, lp, lo) From c5b438baa6f2746ea5885ea7d8b4668765747a4a Mon Sep 17 00:00:00 2001 From: Valeri Atamaniouk Date: Wed, 17 Feb 2016 13:05:33 +0200 Subject: [PATCH 08/18] l2c: added cython bindings for CNAV message decoding Added cython bindings for GPS CNAV message decoding. Fixed destination directory for libfec headers. Added local user environment support for extensions build. Updated CNAV message generation. --- libfec/src/CMakeLists.txt | 2 +- python/setup.py | 29 ++++-- python/swiftnav/cnav_msg.pxd | 36 +++++++ python/swiftnav/cnav_msg.pyx | 95 +++++++++++++++++++ python/swiftnav/edc.pxd | 2 + .../libl2cbitstream/l2cbitstream.c | 37 ++++---- .../libl2cbitstream/l2cbitstream.h | 2 +- 7 files changed, 173 insertions(+), 30 deletions(-) create mode 100644 python/swiftnav/cnav_msg.pxd create mode 100644 python/swiftnav/cnav_msg.pyx diff --git a/libfec/src/CMakeLists.txt b/libfec/src/CMakeLists.txt index 9bc4d638..c690d24c 100644 --- a/libfec/src/CMakeLists.txt +++ b/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/) diff --git a/python/setup.py b/python/setup.py index 53f0fafc..a9f2827d 100755 --- a/python/setup.py +++ b/python/setup.py @@ -24,19 +24,33 @@ 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('.') 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 = [ @@ -44,6 +58,7 @@ def make_extension(ext_name): 'swiftnav.signal', 'swiftnav.coord_system', 'swiftnav.constants', + 'swiftnav.cnav_msg', 'swiftnav.nav_msg', 'swiftnav.pvt', 'swiftnav.correlate', diff --git a/python/swiftnav/cnav_msg.pxd b/python/swiftnav/cnav_msg.pxd new file mode 100644 index 00000000..04c7659e --- /dev/null +++ b/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 diff --git a/python/swiftnav/cnav_msg.pyx b/python/swiftnav/cnav_msg.pyx new file mode 100644 index 00000000..e478a0a3 --- /dev/null +++ b/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:] diff --git a/python/swiftnav/edc.pxd b/python/swiftnav/edc.pxd index 5b8e496b..8fb091e7 100644 --- a/python/swiftnav/edc.pxd +++ b/python/swiftnav/edc.pxd @@ -8,6 +8,8 @@ # WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. from common cimport * +from libcpp cimport bool cdef extern from "libswiftnav/edc.h": u32 crc24q(const u8 *buf, u32 len, u32 crc) + u32 crc24q_bits(u32 crc, const u8 *buf, u32 n_bits, bool invert) diff --git a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c index fcbddf95..a7c84d0a 100644 --- a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c +++ b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.c @@ -31,8 +31,7 @@ #define CNAVMSG_CRC_LEN_BITS (BITS_IN_BYTE * CNAVMSG_CRC_LEN_BYTES) #define CNAVMSG_CRC_IDX_BITS 276 #define CNAVMSG_TOW_COUNT_IDX_BITS 20 -#define CNAVMSG_TOW_COUNT_INC 8 // equals 12 seconds -#define CNAVMSG_TOW_COUNT_MAX 403199 +#define CNAVMSG_TOW_COUNT_INC 2 // equals 12 seconds #define CNAVMSG_TOW_COUNT_LEN_BITS 17 #define CNAVMSG_TOW_COUNT_IGNORE_LSB 2 @@ -47,27 +46,19 @@ u8 get_l2c_message_length(void) * and CRC-24q. Bits are right-aligned so four leftmost bits * of the 38 byte buffer are always zero. * - * \param au_message Buffer for the message. + * \param[out] au_message Buffer for the message. + * \param[in] prn GPS SV PRN number [0..31]. + * \param[in] msg_id Message idntifier [0..31]. + * \param[in] tow ToW in 6 second units [0..131071]. * \return true if success * false if error */ -bool get_l2c_message(u8 *au_message) +bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow) { u32 q_idx = CNAVMSG_OFFSET_BITS; - static u32 q_towcount = 0; - u32 q_towcount_trunc = 0; memset(au_message, 0, CNAVMSG_LEN_BYTES); - q_towcount += CNAVMSG_TOW_COUNT_INC; - - if (CNAVMSG_TOW_COUNT_MAX < q_towcount) - { - q_towcount = 0; - } - - srand(time(NULL) * clock()); - /* * Offset is 4 -> 4 leftmost bits are always '0'. * Preamble. @@ -76,16 +67,15 @@ bool get_l2c_message(u8 *au_message) q_idx += CNAVMSG_PREAMBLE_LEN_BITS; /* PRN */ - setbitu(au_message, q_idx, CNAVMSG_PRN_LEN_BITS, rand()); + setbitu(au_message, q_idx, CNAVMSG_PRN_LEN_BITS, prn); q_idx += CNAVMSG_PRN_LEN_BITS; /* Message Type */ - setbitu(au_message, q_idx, CNAVMSG_TYPE_LEN_BITS, rand()); + setbitu(au_message, q_idx, CNAVMSG_TYPE_LEN_BITS, msg_id); q_idx += CNAVMSG_TYPE_LEN_BITS; /* 17 MSBs of the TOW count -> ignore 2 LSBs */ - q_towcount_trunc = q_towcount >> CNAVMSG_TOW_COUNT_IGNORE_LSB; - setbitu(au_message, q_idx, CNAVMSG_TOW_COUNT_LEN_BITS, q_towcount_trunc); + setbitu(au_message, q_idx, CNAVMSG_TOW_COUNT_LEN_BITS, tow); q_idx += CNAVMSG_TOW_COUNT_LEN_BITS; /* Random payload */ @@ -116,13 +106,15 @@ static s32 get_aligned_message(u8 *au_msg_write) static bool b_offset = true; static u8 au_msg_nxt[CNAVMSG_LEN_BYTES] = {0}; static u8 u_idx = CNAVMSG_LEN_BYTES; + u32 q_tow_count = 0; u64 t_msg_amount = 0; /* First call */ if (u_idx >= CNAVMSG_LEN_BYTES) { - if (!get_l2c_message(au_msg_nxt)) { + if (!get_l2c_message(au_msg_nxt, rand(), rand(), q_tow_count)) { return -1; } else { + q_tow_count += CNAVMSG_TOW_COUNT_INC; u_idx = 0; } } @@ -137,9 +129,10 @@ static s32 get_aligned_message(u8 *au_msg_write) if (u_idx >= CNAVMSG_LEN_BYTES) { /* get next 300 bit message since the buffer is empty*/ - if (!get_l2c_message(au_msg_nxt)) { + if (!get_l2c_message(au_msg_nxt, rand(), rand(), q_tow_count)) { return -1; } else { + q_tow_count += CNAVMSG_TOW_COUNT_INC; u_idx = 0; t_msg_amount++; if (b_offset) { @@ -179,6 +172,8 @@ s32 write_l2c_to_file(int i_fileno, u64 t_wanted_msg_amount) u8 au_msg_write[CNAVMSG_LEN_BYTES] = {0}; FILE *pz_file = fdopen(i_fileno, "wb"); + srand(time(NULL) * clock()); + if (NULL == pz_file) { printf("ERROR opening file\n"); return -1; diff --git a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h index 54ccd8c7..19492c2e 100644 --- a/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h +++ b/tests/data/l2cbitstream/libl2cbitstream/l2cbitstream.h @@ -18,5 +18,5 @@ #define CNAVMSG_LEN_BYTES 38 u8 get_l2c_message_length(void); -bool get_l2c_message(u8 *au_message); +bool get_l2c_message(u8 *au_message, u8 prn, u8 msg_id, u32 tow); s32 write_l2c_to_file(int i_fileno, u64 t_msg_amount); From e89094d81fa8a3f5e4313ecdd18d43e52d61eb20 Mon Sep 17 00:00:00 2001 From: Valeri Atamaniouk Date: Fri, 19 Feb 2016 20:24:43 +0200 Subject: [PATCH 09/18] iqgen: updated cython bindings for l1ca Updated cython bindings for GPS L1 C/A message generation. --- python/swiftnav/bits.pyx | 17 +++++++++++++++-- python/swiftnav/ephemeris.pyx | 19 +++++++++++++------ python/swiftnav/nav_msg.pxd | 1 + python/swiftnav/nav_msg.pyx | 7 +++---- 4 files changed, 32 insertions(+), 12 deletions(-) diff --git a/python/swiftnav/bits.pyx b/python/swiftnav/bits.pyx index 2b7b44a9..4fe0d555 100644 --- a/python/swiftnav/bits.pyx +++ b/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 diff --git a/python/swiftnav/ephemeris.pyx b/python/swiftnav/ephemeris.pyx index 35a3dc81..311bc1d6 100644 --- a/python/swiftnav/ephemeris.pyx +++ b/python/swiftnav/ephemeris.pyx @@ -23,12 +23,19 @@ cdef class Ephemeris: def __init__(self, **kwargs): memset(&self._thisptr, 0, sizeof(ephemeris_t)) - self._thisptr.sid = kwargs.pop('sid') - self._thisptr.toe = kwargs.pop('toe') - self._thisptr.ura = kwargs.pop('ura') - self._thisptr.fit_interval = kwargs.pop('fit_interval') - self._thisptr.valid = kwargs.pop('valid') - self._thisptr.healthy = kwargs.pop('healthy') + + if 'sid' in kwargs: + self._thisptr.sid = kwargs.pop('sid') + if 'toe' in kwargs: + self._thisptr.toe = kwargs.pop('toe') + if 'ura' in kwargs: + self._thisptr.ura = kwargs.pop('ura') + if 'fit_interval' in kwargs: + self._thisptr.fit_interval = kwargs.pop('fit_interval') + if 'valid' in kwargs: + self._thisptr.valid = kwargs.pop('valid') + if 'healthy' in kwargs: + self._thisptr.healthy = kwargs.pop('healthy') if 'kepler' in kwargs: self._thisptr.kepler = kwargs.pop('kepler') elif 'xyz' in kwargs: diff --git a/python/swiftnav/nav_msg.pxd b/python/swiftnav/nav_msg.pxd index d51ab325..61c758a1 100644 --- a/python/swiftnav/nav_msg.pxd +++ b/python/swiftnav/nav_msg.pxd @@ -37,3 +37,4 @@ cdef extern from "libswiftnav/nav_msg.h": cdef class NavMsg: cdef nav_msg_t _thisptr + diff --git a/python/swiftnav/nav_msg.pyx b/python/swiftnav/nav_msg.pyx index f2ddf1db..308ef4aa 100644 --- a/python/swiftnav/nav_msg.pyx +++ b/python/swiftnav/nav_msg.pyx @@ -9,7 +9,7 @@ # cython: embedsignature=True -from ephemeris cimport ephemeris_t +from swiftnav.ephemeris import Ephemeris from fmt_utils import fmt_repr from time cimport gps_time_t from signal cimport gnss_signal_t @@ -40,9 +40,8 @@ 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, Ephemeris e): + return process_subframe(&self._thisptr, &e._thisptr) def __richcmp__(self, other, op): """ From eaefe03148aa7e966ce6c1c789ba132109cfb977 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 22 Feb 2016 11:19:11 +0200 Subject: [PATCH 10/18] Add alias lock detectors reinit() --- python/swiftnav/track.pxd | 1 + python/swiftnav/track.pyx | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/python/swiftnav/track.pxd b/python/swiftnav/track.pxd index e5e23b67..8ae42763 100644 --- a/python/swiftnav/track.pxd +++ b/python/swiftnav/track.pxd @@ -124,6 +124,7 @@ cdef extern from "libswiftnav/track.h": void alias_detect_init(alias_detect_t *a, u32 acc_len, float time_diff) void alias_detect_first(alias_detect_t *a, float I, float Q) float alias_detect_second(alias_detect_t *a, float I, float Q) + void alias_detect_reinit(alias_detect_t *a, u32 acc_len, float time_diff) # Tracking loop: Lock detect struct loop_detect_lpf: diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index 282fb979..c9ae0aec 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -377,7 +377,9 @@ cdef class LockDetector: def update(self, I, Q, DT): lock_detect_update(&self._thisptr, I, Q, DT) - return (self._thisptr.outo, self._thisptr.outp) + return (self._thisptr.outo, self._thisptr.outp, \ + self._thisptr.pcount1, self._thisptr.pcount2,\ + self._thisptr.lpfi.y, self._thisptr.lpfq.y) cdef class AliasDetector: @@ -391,6 +393,9 @@ cdef class AliasDetector: def second(self, I, Q): return alias_detect_second(&self._thisptr, I, Q) + def reinit(self, acc_len, time_diff): + return alias_detect_reinit(&self._thisptr, acc_len, time_diff) + cdef class CN0Estimator: """ Wraps the `libswiftnav` :math:`C / N_0` estimator implementation. From 8a10026a694cd373fed1ecd6ea12d0d295f85641 Mon Sep 17 00:00:00 2001 From: Valeri Atamaniouk Date: Tue, 15 Mar 2016 14:47:05 +0200 Subject: [PATCH 11/18] tracking: fixed python bindings for aided tracking Updated python implementation for aided tracking. --- python/swiftnav/track.pxd | 4 ++-- python/swiftnav/track.pyx | 16 +++++++--------- 2 files changed, 9 insertions(+), 11 deletions(-) diff --git a/python/swiftnav/track.pxd b/python/swiftnav/track.pxd index 8ae42763..6f28daaa 100644 --- a/python/swiftnav/track.pxd +++ b/python/swiftnav/track.pxd @@ -200,7 +200,7 @@ cdef class SimpleTrackingLoop: cdef simple_tl_state_t _thisptr cdef class AidedLoopFilter: - cdef aided_lf_state_t _thisptr + cdef aided_lf_state_t _thisptr cdef class AidedTrackingLoop: cdef aided_tl_state_t _thisptr @@ -224,6 +224,6 @@ cdef class ChannelMeasurement: cdef channel_measurement_t _thisptr cdef class NavigationMeasurement: - cdef navigation_measurement_t _thisptr + cdef navigation_measurement_t _thisptr cdef mk_nav_meas_array(py_nav_meas, u8 n_c_nav_meas, navigation_measurement_t *c_nav_meas) diff --git a/python/swiftnav/track.pyx b/python/swiftnav/track.pyx index c9ae0aec..512bb357 100644 --- a/python/swiftnav/track.pyx +++ b/python/swiftnav/track.pyx @@ -266,15 +266,13 @@ cdef class AidedTrackingLoop: FLL aiding gain """ - self.loop_freq = loop_freq - self.code_bw, self.code_zeta, self.code_k = code_params - self.carr_bw, self.carr_zeta, self.carr_k = carr_params - self.carr_freq_igain = carr_freq_igain - aided_tl_retune(&self._thisptr, self.loop_freq, - self.code_bw, self.code_zeta, self.code_k, - self.carr_to_code, - self.carr_bw, self.carr_zeta, self.carr_k, - self.carr_freq_igain) + code_bw, code_zeta, code_k = code_params + carr_bw, carr_zeta, carr_k = carr_params + aided_tl_retune(&self._thisptr, loop_freq, + code_bw, code_zeta, code_k, + carr_to_code, + carr_bw, carr_zeta, carr_k, + carr_freq_igain) def update(self, E, P, L): """ From 204ced219d3b65c9b11286b7e680183971b6282b Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Fri, 11 Mar 2016 14:11:59 +0200 Subject: [PATCH 12/18] Add counter checkers for counters in RF4&RF1 and RF3&RF2 --- include/libswiftnav/counter_checker.h | 121 ++++++++++ src/CMakeLists.txt | 1 + src/counter_checker/Makefile | 12 + src/counter_checker/counter_checker.c | 306 ++++++++++++++++++++++++++ tests/CMakeLists.txt | 1 + tests/check_counter_checker.c | 213 ++++++++++++++++++ tests/check_main.c | 1 + tests/check_suites.h | 1 + 8 files changed, 656 insertions(+) create mode 100644 include/libswiftnav/counter_checker.h create mode 100644 src/counter_checker/Makefile create mode 100644 src/counter_checker/counter_checker.c create mode 100644 tests/check_counter_checker.c diff --git a/include/libswiftnav/counter_checker.h b/include/libswiftnav/counter_checker.h new file mode 100644 index 00000000..09f4b5c7 --- /dev/null +++ b/include/libswiftnav/counter_checker.h @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Adel Mamin + * + * 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 +#include +#include + +/** 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 */ diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3258c1f5..67348bef 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -46,6 +46,7 @@ set(libswiftnav_SRCS ionosphere.c bit_sync.c cnav_msg.c + counter_checker/counter_checker.c ${plover_SRCS} CACHE INTERNAL "" diff --git a/src/counter_checker/Makefile b/src/counter_checker/Makefile new file mode 100644 index 00000000..91e1e931 --- /dev/null +++ b/src/counter_checker/Makefile @@ -0,0 +1,12 @@ +.PHONY: all clean + +all: counter_checker_rf41 counter_checker_rf32 + +counter_checker_rf41: counter_checker.c + gcc -o counter_checker_rf41 -O3 -Wall -I ../../include -D RF41 -D STANDALONE_SETUP counter_checker.c + +counter_checker_rf32: counter_checker.c + gcc -o counter_checker_rf32 -O3 -Wall -I ../../include -D RF32 -D STANDALONE_SETUP counter_checker.c + +clean: + rm counter_checker_rf32 counter_checker_rf41 diff --git a/src/counter_checker/counter_checker.c b/src/counter_checker/counter_checker.c new file mode 100644 index 00000000..65807c03 --- /dev/null +++ b/src/counter_checker/counter_checker.c @@ -0,0 +1,306 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Adel Mamin + * + * 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 is one byte, which is encoded + lile this: + + RF4 RF3 RF2 RF1 + 00 00 00 00 + + , where + + RF1 - GPS L1 + RF2 - GLONASS L1 + RF3 - GLONASS L2 + RF4 - GPS L2 + + This utility expects 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 of the counter + and RF3 - two most significant bits. + Therefore, the bit indexes are: + RF3 RF2 + 5&4 3&2 +*/ + +#include +#include +#include + +/** The data is read in chunks, which are stored here. */ +static uint32_t chunk[SAMPLE_CHUNK_SIZE / sizeof(uint32_t)]; + +/** Get array size */ +#define ARR_SIZE(arr) (sizeof(arr) / sizeof((arr)[0])) + +/** Mismatch data */ +static struct mismatch_data mismatch; + +/** Checks counter mismatch. + * + * \param data_offset File offset in bytes. + * \param bytes The data bytes to check. + * \param byte_index The byte index within \e bytes parameter [0,1,2,3] + * \param expected_counter Next expected counter value. + * + * \return The next expected counter value + */ +static inline uint8_t check_counter(size_t data_offset, + uint8_t (*get_counter)(uint8_t data), + uint32_t bytes, uint8_t byte_index, + uint32_t expected_counter) +{ + uint8_t actual_counter; + uint8_t byte_offset = 8 * byte_index; + + actual_counter = get_counter(bytes >> byte_offset); + if (actual_counter != expected_counter) { + if (mismatch.counter <= ARR_SIZE(mismatch.data)) { + mismatch.data[mismatch.counter].offset = data_offset; + mismatch.data[mismatch.counter].data = (bytes >> 8 * byte_index) & 0xFF; + mismatch.data[mismatch.counter].expected_counter = expected_counter; + mismatch.data[mismatch.counter].actual_counter = actual_counter; + mismatch.counter++; + } + } + expected_counter = (actual_counter + 1) % SAMPLE_COUNTER_MODULO; + + return expected_counter; +} + +/** Report counter mismatch issues. */ +void report_mismatch(const struct mismatch_data *mismatch) +{ + size_t i; + size_t offset_prev = mismatch->data[0].offset; + + if (0 != mismatch->counter) { + printf("data_offset,data,expected_counter,actual_counter,offset_diff\n"); + } + for (i = 0; i < mismatch->counter; i++) { + printf("%ld,", mismatch->data[i].offset); + printf("0x%02X,", mismatch->data[i].data); + printf("%d,", mismatch->data[i].expected_counter); + printf("%d,", mismatch->data[i].actual_counter); + printf("%ld\n", mismatch->data[i].offset - offset_prev); + offset_prev = mismatch->data[i].offset; + } +} + +/** Get RF3 RF2 counter + * \param data Data byte + * \return The counter value + */ +uint8_t get_rf32_counter(uint8_t data) +{ + uint8_t counter = GET_BITS(data, 2, 4); + return counter; +} + +/** Set RF3 RF2 counter + * \param data Data byte + * \param counter The counter value + * \return The data byte with embedded counter + */ +uint8_t set_rf32_counter(uint8_t data, uint8_t counter) +{ + data = SET_BITS(data, 2, 4, counter); + return data; +} + +/** Get RF4 RF1 counter + * \param data Data byte + * \return The counter value + */ +uint8_t get_rf41_counter(uint8_t data) +{ + uint8_t counter; + + // take RF4 part first + counter = GET_BITS(data, 6, 2) << 2; + // combine with RF1 part to get full counter + counter |= GET_BITS(data, 0, 2); + + return counter; +} + +/** Set RF4 RF1 counter + * \param data Data byte + * \param counter The counter value + * \return The data byte with embedded counter + */ +uint8_t set_rf41_counter(uint8_t data, uint8_t counter) +{ + data = SET_BITS(data, 0, 2, counter); + counter >>= 2; + data = SET_BITS(data, 6, 2, counter); + + return data; +} + +/** Initialize sample checker */ +void counter_checker_init(void) +{ + mismatch.counter = 0; +} + +/** Runs sample checker + * + * \param csb Callbacks for data reading and counter extraction + * \param stream Stream itself. + * \param data_size Total data size [bytes] + * \return Mismatch data pointer. Non NULL even if no mismatch found. + The memory pointer is owned by the callee. Do not free it! + */ +const struct mismatch_data *counter_checker_run(struct callbacks *cbs, + void *stream, + size_t data_size) +{ + size_t i; + size_t j; + size_t chunk_size; + size_t chunk_num; + uint8_t tmp; + uint8_t counter; + size_t data_offset; + + /* initialize the counter from the first byte of the samples data */ + chunk_size = cbs->read(&tmp, 1, stream); + counter = cbs->get_counter(tmp); + cbs->rewind(stream); + + data_offset = 0; + + /* now let's start the integrity check of the samples data + in chunks of size SAMPLE_CHUNK_SIZE */ + chunk_num = data_size / sizeof(chunk); + + /* printf("chunk_num = %ld\n", chunk_num); */ + + for (i = 0; i < chunk_num; i++) { + chunk_size = cbs->read(chunk, sizeof(chunk), stream); + + /* process the chunk in 4 bytes slices */ + for (j = 0; j < chunk_size / sizeof(chunk[0]); j++) { + uint8_t k; + + for (k = 0; k < sizeof(uint32_t); k++) { + counter = check_counter(data_offset, cbs->get_counter, + chunk[j], k, counter); + data_offset++; + } + } + } + + chunk_size = data_size % sizeof(chunk); + chunk_size = cbs->read(chunk, chunk_size, stream); + + /* printf("chunk_size = %ld\n", chunk_size); */ + + /* process the last chunk byte by byte */ + for (j = 0; j < chunk_size; j++) { + counter = check_counter(data_offset, cbs->get_counter, + ((uint8_t*)chunk)[j], 0, counter); + data_offset++; + } + + return &mismatch; +} + +#ifdef STANDALONE_SETUP + +/** Standard C library 'fread' function wrapper */ +static size_t file_read(void *ptr, size_t size, void *f) +{ + size_t ret; + ret = fread(ptr, size, 1, (FILE*)f); + return ret * size; +} + +/** Standard C library 'rewind' function wrapper */ +static void file_rewind(void *f) +{ + rewind((FILE*)f); +} + +static struct callbacks cbs = { + + file_read, + file_rewind, + +#if defined RF41 + get_rf41_counter, + set_rf41_counter, +#elif defined RF32 + get_rf32_counter, + set_rf32_counter +#else +# error "Please, specify, where counter bits are located." +#endif + +}; + +int main(int argc, char* argv[]) +{ + FILE *f; + const char* file_name; + long file_size; + int ret = 0; + const struct mismatch_data *mismatch; + + + if (argc < 2) { + printf("Usage:"); + printf("%s \n", argv[0]); + return -1; + } + + file_name = argv[1]; + + f = fopen(file_name, "rb"); + if (NULL == f) { + printf("Failed to open file %s\n", file_name); + return -1; + } + + fseek(f, 0, SEEK_END); + file_size = ftell(f); + rewind(f); + + if (0 == file_size) { + printf("File %s is empty\n", file_name); + ret = -1; + goto end; + } + + counter_checker_init(); + mismatch = counter_checker_run(&cbs, f, file_size); + + if (0 == mismatch->counter) { + printf("The file is OK!\n"); + } else { + report_mismatch(mismatch); + } + + +end: + + fclose(f); + + return ret; +} + +#endif /* #ifdef STANDALONE_SETUP */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e6939768..f1d8718a 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -48,6 +48,7 @@ else (CMAKE_CROSSCOMPILING) check_signal.c check_track.c check_cnav.c + check_counter_checker.c ) target_link_libraries(test_libswiftnav ${TEST_LIBS}) diff --git a/tests/check_counter_checker.c b/tests/check_counter_checker.c new file mode 100644 index 00000000..3cc79c62 --- /dev/null +++ b/tests/check_counter_checker.c @@ -0,0 +1,213 @@ +#include +#include +#include +#include + +struct recording { + u8* samples; + size_t size; +}; + +static size_t arr_offset = 0; + +static size_t arr_read(void *ptr, size_t size, void *f) +{ + memcpy(ptr, (u8*) f + arr_offset, size); + arr_offset += size; + return size; +} + +static void arr_rewind(void *f) +{ + (void)f; + arr_offset = 0; +} + +static struct recording generate_recording(u8 counter_start, + uint8_t set_counter(uint8_t data, uint8_t counter), + u8 counter_modulo, + size_t size) +{ + size_t i; + u8* samples; + u8 counter; + struct recording recording = {NULL, 0}; + + samples = malloc(size); + if (NULL == samples) { + return recording; + } + + counter = counter_start; + for (i = 0; i < size; i++) { + samples[i] = set_counter(samples[i], counter); + counter++; + counter %= counter_modulo; + } + recording.samples = samples; + recording.size = size; + + return recording; +} + +START_TEST(test_counters_rf41) +{ + u8 data = 0xFF; + u8 i; + u8 counter; + + for (i = 0; i < 16; i++) { + data = set_rf41_counter(data, i); + counter = get_rf41_counter(data); + fail_if(counter != i, "Unexpected counter value"); + } + data = set_rf41_counter(data, 0); + fail_if(0x3C != data, "Unexpected counter value"); +} +END_TEST + +START_TEST(test_counters_rf32) +{ + u8 data = 0xFF; + u8 i; + u8 counter; + + for (i = 0; i < 16; i++) { + data = set_rf32_counter(data, i); + counter = get_rf32_counter(data); + fail_if(counter != i, "Unexpected counter value"); + } + data = set_rf32_counter(data, 0); + fail_if(0xC3 != data, "Unexpected counter value"); +} +END_TEST + +START_TEST(test_counter_checker_rf41) +{ + size_t i; + struct recording recording; + const struct mismatch_data *mismatch; + size_t size; + size_t disc_counter; + + static struct callbacks cbs = { + arr_read, + arr_rewind, + get_rf41_counter, + set_rf41_counter, + }; + + // check sample data with different counter values + size = SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2; + for (i = 0; i < SAMPLE_COUNTER_MODULO; i++) { + recording = generate_recording(i, set_rf41_counter, + SAMPLE_COUNTER_MODULO, + SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2); + fail_if(NULL == recording.samples, "Could not allocate recording data"); + fail_if(size != recording.size, "Recording data size mismatch"); + + arr_offset = 0; + counter_checker_init(); + mismatch = counter_checker_run(&cbs, recording.samples, recording.size); + fail_if(NULL == mismatch, "Unexpected return value"); + fail_if(mismatch->counter != 0, "Unexpected counter mismatch found"); + free(recording.samples); + } + + // introduce counter discontinuity + size = 2 * SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2; + recording = generate_recording(0, set_rf41_counter, SAMPLE_COUNTER_MODULO, + size); + fail_if(NULL == recording.samples, "Could not allocate recording data"); + fail_if(size != recording.size, "Recording data size mismatch"); + + disc_counter = 0; + for (i = 1; i < size; i += size / 1024) { + u8 counter = get_rf41_counter(recording.samples[i - 1]); + + counter += 2; + counter %= SAMPLE_COUNTER_MODULO; + recording.samples[i] = set_rf41_counter(recording.samples[i], counter); + + disc_counter++; + } + + arr_offset = 0; + counter_checker_init(); + mismatch = counter_checker_run(&cbs, recording.samples, recording.size); + + fail_if(mismatch->counter != 2 * disc_counter, "Counter mismatch failure"); +} +END_TEST + +START_TEST(test_counter_checker_rf32) +{ + size_t i; + struct recording recording; + const struct mismatch_data *mismatch; + size_t size; + size_t disc_counter; + + static struct callbacks cbs = { + arr_read, + arr_rewind, + get_rf32_counter, + set_rf32_counter, + }; + + // check sample data with different counter values + size = SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2; + for (i = 0; i < SAMPLE_COUNTER_MODULO; i++) { + recording = generate_recording(i, set_rf32_counter, + SAMPLE_COUNTER_MODULO, + SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2); + fail_if(NULL == recording.samples, "Could not allocate recording data"); + fail_if(size != recording.size, "Recording data size mismatch"); + + arr_offset = 0; + counter_checker_init(); + mismatch = counter_checker_run(&cbs, recording.samples, recording.size); + fail_if(NULL == mismatch, "Unexpected return value"); + fail_if(mismatch->counter != 0, "Unexpected counter mismatch found"); + free(recording.samples); + } + + // introduce counter discontinuity + size = 2 * SAMPLE_CHUNK_SIZE + SAMPLE_CHUNK_SIZE / 2; + recording = generate_recording(0, set_rf32_counter, SAMPLE_COUNTER_MODULO, + size); + fail_if(NULL == recording.samples, "Could not allocate recording data"); + fail_if(size != recording.size, "Recording data size mismatch"); + + disc_counter = 0; + for (i = 1; i < size; i += size / 1024) { + u8 counter = get_rf32_counter(recording.samples[i - 1]); + + counter += 2; + counter %= SAMPLE_COUNTER_MODULO; + recording.samples[i] = set_rf32_counter(recording.samples[i], counter); + + disc_counter++; + } + + arr_offset = 0; + counter_checker_init(); + mismatch = counter_checker_run(&cbs, recording.samples, recording.size); + + fail_if(mismatch->counter != 2 * disc_counter, "Counter mismatch failure"); +} +END_TEST + +Suite* counter_checker_suite(void) +{ + Suite *s = suite_create("Counter checker"); + TCase *tc_core = tcase_create("Core"); + + tcase_add_test(tc_core, test_counters_rf32); + tcase_add_test(tc_core, test_counters_rf41); + tcase_add_test(tc_core, test_counter_checker_rf41); + tcase_add_test(tc_core, test_counter_checker_rf32); + suite_add_tcase(s, tc_core); + + return s; +} diff --git a/tests/check_main.c b/tests/check_main.c index 0f937e23..6603318d 100644 --- a/tests/check_main.c +++ b/tests/check_main.c @@ -34,6 +34,7 @@ int main(void) srunner_add_suite(sr, track_test_suite()); srunner_add_suite(sr, cnav_test_suite()); srunner_add_suite(sr, correlator_suite()); + srunner_add_suite(sr, counter_checker_suite()); srunner_set_fork_status(sr, CK_NOFORK); srunner_run_all(sr, CK_NORMAL); diff --git a/tests/check_suites.h b/tests/check_suites.h index c572242a..3994ba1c 100644 --- a/tests/check_suites.h +++ b/tests/check_suites.h @@ -24,5 +24,6 @@ Suite* signal_test_suite(void); Suite* track_test_suite(void); Suite* cnav_test_suite(void); Suite* correlator_suite(void); +Suite* counter_checker_suite(void); #endif /* CHECK_SUITES_H */ From bb45031436864520c4005cddcec792cb2c2b7e11 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Fri, 18 Mar 2016 10:28:33 +0200 Subject: [PATCH 13/18] Fix printf for size_t vars --- src/counter_checker/counter_checker.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/counter_checker/counter_checker.c b/src/counter_checker/counter_checker.c index 65807c03..06095f49 100644 --- a/src/counter_checker/counter_checker.c +++ b/src/counter_checker/counter_checker.c @@ -91,11 +91,11 @@ void report_mismatch(const struct mismatch_data *mismatch) printf("data_offset,data,expected_counter,actual_counter,offset_diff\n"); } for (i = 0; i < mismatch->counter; i++) { - printf("%ld,", mismatch->data[i].offset); + printf("%zu,", mismatch->data[i].offset); printf("0x%02X,", mismatch->data[i].data); printf("%d,", mismatch->data[i].expected_counter); printf("%d,", mismatch->data[i].actual_counter); - printf("%ld\n", mismatch->data[i].offset - offset_prev); + printf("%zu\n", mismatch->data[i].offset - offset_prev); offset_prev = mismatch->data[i].offset; } } From ed093deae4beb62bdbad8498fd690881b724a0e4 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Tue, 22 Mar 2016 08:52:01 +0200 Subject: [PATCH 14/18] Fix travis build --- python/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/setup.py b/python/setup.py index a9f2827d..9ab8c3b0 100755 --- a/python/setup.py +++ b/python/setup.py @@ -43,6 +43,7 @@ include_dirs.append(np.get_include()) include_dirs.append(os.path.expanduser('~/.local/include')) include_dirs.append('.') + include_dirs.append('../include/') def make_extension(ext_name): ext_path = ext_name.replace('.', os.path.sep) + '.pyx' return Extension( From 383972dde3c7f492e4180180b293453ca8e9f863 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Tue, 22 Mar 2016 09:16:04 +0200 Subject: [PATCH 15/18] Fix travis build --- python/setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/python/setup.py b/python/setup.py index 9ab8c3b0..a6afc1c4 100755 --- a/python/setup.py +++ b/python/setup.py @@ -44,6 +44,7 @@ include_dirs.append(os.path.expanduser('~/.local/include')) include_dirs.append('.') include_dirs.append('../include/') + include_dirs.append('../libfec/include/') def make_extension(ext_name): ext_path = ext_name.replace('.', os.path.sep) + '.pyx' return Extension( From 19a3718005fc4c8a8e2fe4a60c381bb5829e34c5 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Tue, 22 Mar 2016 11:40:53 +0200 Subject: [PATCH 16/18] Fix travis build --- python/setup.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/python/setup.py b/python/setup.py index a6afc1c4..ab0514ea 100755 --- a/python/setup.py +++ b/python/setup.py @@ -43,8 +43,10 @@ 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' return Extension( From d96b4e51a5788d66b4cb1eaff26e500c85b529a7 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Tue, 5 Apr 2016 09:43:15 +0300 Subject: [PATCH 17/18] Fix doxygen comments --- src/correlate.c | 12 ++++++------ src/counter_checker/counter_checker.c | 6 +----- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/correlate.c b/src/correlate.c index 4ad75c3e..16dbca45 100644 --- a/src/correlate.c +++ b/src/correlate.c @@ -47,11 +47,11 @@ static void track_correlate(enum correlator_type correlator_type, * \param samples Samples array. One byte per sample. * \param samples_len Samples array size. * \param code L1C/A PRN code. One byte per chip: 1023 bytes long. - * \param[in/out] init_code_phase Initial code phase [chips]. + * \param[in,out] init_code_phase Initial code phase [chips]. * The function returns the * the last unprocessed code phase here. * \param code_step Code phase increment step [chips]. - * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * \param[in,out] init_carr_phase Initial carrier phase [radians]. * The function returns the the last unprocessed carrier * phase here. * \param carr_step Carrier phase increment step [radians]. @@ -96,11 +96,11 @@ void l1_ca_track_correlate(const s8* restrict samples, size_t samples_len, * \param samples Samples array. One byte per sample. * \param samples_len Samples array size. * \param code L2C CM PRN code. One byte per chip: 10230 bytes long. - * \param[in/out] init_code_phase Initial code phase [chips]. + * \param[in,out] init_code_phase Initial code phase [chips]. * The function returns the * the last unprocessed code phase here. * \param code_step Code phase increment step [chips]. - * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * \param[in,out] init_carr_phase Initial carrier phase [radians]. * The function returns the the last unprocessed carrier * phase here. * \param carr_step Carrier phase increment step [radians]. @@ -216,11 +216,11 @@ static inline double l2c_cm_get_code_phase(double code_phase, double code_step) * \param correlator_type Correlator type. L1 C/A or L2C CM are supported. * \param samples Samples array. One byte per sample. * \param code PRN code. One byte per chip. - * \param[in/out] init_code_phase Initial code phase [chips]. + * \param[in,out] init_code_phase Initial code phase [chips]. * The function returns the last unprocessed code * phase here. * \param code_step Code phase increment step [chips]. - * \param[in/out] init_carr_phase Initial carrier phase [radians]. + * \param[in,out] init_carr_phase Initial carrier phase [radians]. * The function returns the the last unprocessed carrier * phase here. * \param carr_step Carrier phase increment step [radians]. diff --git a/src/counter_checker/counter_checker.c b/src/counter_checker/counter_checker.c index 06095f49..458006ff 100644 --- a/src/counter_checker/counter_checker.c +++ b/src/counter_checker/counter_checker.c @@ -159,7 +159,7 @@ void counter_checker_init(void) /** Runs sample checker * - * \param csb Callbacks for data reading and counter extraction + * \param cbs Callbacks for data reading and counter extraction * \param stream Stream itself. * \param data_size Total data size [bytes] * \return Mismatch data pointer. Non NULL even if no mismatch found. @@ -188,8 +188,6 @@ const struct mismatch_data *counter_checker_run(struct callbacks *cbs, in chunks of size SAMPLE_CHUNK_SIZE */ chunk_num = data_size / sizeof(chunk); - /* printf("chunk_num = %ld\n", chunk_num); */ - for (i = 0; i < chunk_num; i++) { chunk_size = cbs->read(chunk, sizeof(chunk), stream); @@ -208,8 +206,6 @@ const struct mismatch_data *counter_checker_run(struct callbacks *cbs, chunk_size = data_size % sizeof(chunk); chunk_size = cbs->read(chunk, chunk_size, stream); - /* printf("chunk_size = %ld\n", chunk_size); */ - /* process the last chunk byte by byte */ for (j = 0; j < chunk_size; j++) { counter = check_counter(data_offset, cbs->get_counter, From 0a7c4943f0b3f208d136319de6b9b246fd55c0a0 Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Fri, 1 Apr 2016 16:32:59 +0300 Subject: [PATCH 18/18] Add Glonass CA correlator --- include/libswiftnav/correlate.h | 11 +++ python/swiftnav/correlate.pxd | 9 +++ python/swiftnav/correlate.pyx | 5 ++ src/correlate.c | 117 ++++++++++++++++++++++++++++++-- tests/check_correlator.c | 114 +++++++++++++++++++++++++++---- 5 files changed, 238 insertions(+), 18 deletions(-) diff --git a/include/libswiftnav/correlate.h b/include/libswiftnav/correlate.h index 14cebfd3..1df62828 100644 --- a/include/libswiftnav/correlate.h +++ b/include/libswiftnav/correlate.h @@ -32,4 +32,15 @@ void l2c_cm_track_correlate(const s8* samples, size_t samples_len, double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples); +void glo_ca_track_correlate(const s8* samples, size_t samples_len, + const 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); + #endif /* LIBSWIFTNAV_CORRELATE_H */ diff --git a/python/swiftnav/correlate.pxd b/python/swiftnav/correlate.pxd index a593d427..f4bb3e0d 100644 --- a/python/swiftnav/correlate.pxd +++ b/python/swiftnav/correlate.pxd @@ -26,3 +26,12 @@ cdef extern from "libswiftnav/correlate.h": double* I_P, double* Q_P, double* I_L, double* Q_L, u32* num_samples) + + void glo_ca_track_correlate(s8* samples, size_t samples_len, 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); + diff --git a/python/swiftnav/correlate.pyx b/python/swiftnav/correlate.pyx index eafa4533..f39a170b 100644 --- a/python/swiftnav/correlate.pyx +++ b/python/swiftnav/correlate.pyx @@ -43,6 +43,11 @@ def track_correlate(np.ndarray[char, ndim=1, mode="c"] samples, &init_code_phase, code_freq / sampling_freq, &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq, &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize) + elif signal == "gloca": + glo_ca_track_correlate(&samples[0], len(samples), &code[0], + &init_code_phase, code_freq / sampling_freq, + &init_carr_phase, carr_freq * 2.0 * M_PI / sampling_freq, + &I_E, &Q_E, &I_P, &Q_P, &I_L, &Q_L, &blksize) else: l2c_cm_track_correlate(&samples[0], len(samples), &code[0], &init_code_phase, code_freq / sampling_freq, diff --git a/src/correlate.c b/src/correlate.c index 16dbca45..747a6f5b 100644 --- a/src/correlate.c +++ b/src/correlate.c @@ -26,11 +26,13 @@ enum correlator_type { L1CA_CORRELATOR, - L2C_CORRELATOR + L2C_CORRELATOR, + GLOCA_CORRELATOR }; -#define L1_CA_CHIPS_PER_PRN_CODE 1023 -#define L2C_CM_CHIPS_PER_PRN_CODE 10230 +#define L1_CA_CHIPS_PER_PRN_CODE 1023 +#define L2C_CM_CHIPS_PER_PRN_CODE 10230 +#define GLO_CA_CHIPS_PER_PRN_CODE 511 static void track_correlate(enum correlator_type correlator_type, const s8* restrict samples, @@ -140,6 +142,59 @@ void l2c_cm_track_correlate(const s8* samples, size_t samples_len, I_E, Q_E, I_P, Q_P, I_L, Q_L, *num_samples); } +/** Perform GLO CA correlation. + * + * \param samples Samples array. One byte per sample. + * \param samples_len Samples array size. + * \param code L2C CM PRN code. One byte per chip: 10230 bytes long. + * \param[in,out] init_code_phase Initial code phase [chips]. + * The function returns the + * the last unprocessed code phase here. + * \param code_step Code phase increment step [chips]. + * \param[in,out] init_carr_phase Initial carrier phase [radians]. + * The function returns the the last unprocessed carrier + * phase here. + * \param carr_step Carrier phase increment step [radians]. + * \param[out] I_E Early replica in-phase correlation component. + * \param[out] Q_E Early replica quadrature correlation component. + * \param[out] I_P Prompt replica in-phase correlation component. + * \param[out] Q_P Prompt replica quadrature correlation component. + * \param[out] I_L Late replica in-phase correlation component. + * \param[out] Q_L Late replica quadrature correlation component. + * \param[out] num_samples The number of processed samples from \e samples array. + */ +void glo_ca_track_correlate(const s8* samples, size_t samples_len, + const 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) +{ + *num_samples = (int)ceil((2 * GLO_CA_CHIPS_PER_PRN_CODE - + * init_code_phase) / code_step); + + if (0 == *num_samples) { + *num_samples = + (int)ceil(2 * GLO_CA_CHIPS_PER_PRN_CODE / code_step); + } + + if (*num_samples > samples_len) { + *num_samples = samples_len; + } + + if (0 == *num_samples) { + return; + } + + track_correlate(GLOCA_CORRELATOR, samples, code, init_code_phase, + code_step, init_carr_phase, carr_step, I_E, Q_E, I_P, + Q_P, I_L, Q_L, *num_samples); +} + /** Produce L1 C/A chip for the given code phase * * \param code L1 C/A PRN code array. One byte per sample: 1023 bytes long. @@ -209,6 +264,43 @@ static inline double l2c_cm_get_code_phase(double code_phase, double code_step) return code_phase; } +/** Produce GLO C/A chip for the given code phase + * + * \param code GLO C/A PRN code array. + * One byte per sample: 511 bytes long. + * \param code_phase Code phase of the chip to return. + * \return Code chip. + */ +static inline s8 glo_ca_get_chip(const s8 *code, double code_phase) +{ + int i; + if (code_phase < 0) { + i = (int)(code_phase + GLO_CA_CHIPS_PER_PRN_CODE); + } else if (code_phase >= GLO_CA_CHIPS_PER_PRN_CODE) { + i = (int)(code_phase - GLO_CA_CHIPS_PER_PRN_CODE); + } else { + i = (int)code_phase; + } + return code[i]; +} + +/** Produce a new GLO C/A code phase given current code phase + * and the step. + * + * \param code_phase Current code phase [chips]. + * \param code_step Code phase update step [chips]. + * \return New code phase. + */ +static inline double glo_ca_get_code_phase(double code_phase, + double code_step) +{ + code_phase += code_step; + if (code_phase >= GLO_CA_CHIPS_PER_PRN_CODE) { + code_phase -= GLO_CA_CHIPS_PER_PRN_CODE; + } + return code_phase; +} + #ifndef __SSSE3__ /** Perform correlation. @@ -233,6 +325,7 @@ static inline double l2c_cm_get_code_phase(double code_phase, double code_step) * \param num_samples The number of samples to correlate from \e samples * array. */ + static void track_correlate(enum correlator_type correlator_type, const s8* restrict samples, const s8* restrict code, @@ -274,6 +367,12 @@ static void track_correlate(enum correlator_type correlator_type, code_L = l2c_cm_get_chip(code, code_phase + 0.5); code_phase_new = l2c_cm_get_code_phase(code_phase, code_step); break; + case GLOCA_CORRELATOR: + code_E = glo_ca_get_chip(code, code_phase - 0.5); + code_P = glo_ca_get_chip(code, code_phase); + code_L = glo_ca_get_chip(code, code_phase + 0.5); + code_phase_new = glo_ca_get_code_phase(code_phase, code_step); + break; default: break; } @@ -285,7 +384,8 @@ static void track_correlate(enum correlator_type correlator_type, rotation matrix */ double carr_sin_ = carr_sin*cos_delta + carr_cos*sin_delta; double carr_cos_ = carr_cos*cos_delta - carr_sin*sin_delta; - double i_mag = (3.0 - carr_sin_*carr_sin_ - carr_cos_*carr_cos_) / 2.0; + double i_mag = + (3.0 - carr_sin_*carr_sin_ - carr_cos_*carr_cos_) / 2.0; carr_sin = carr_sin_ * i_mag; carr_cos = carr_cos_ * i_mag; @@ -299,7 +399,8 @@ static void track_correlate(enum correlator_type correlator_type, code_phase = code_phase_new; } *init_code_phase = code_phase; - *init_carr_phase = fmod(*init_carr_phase + num_samples * carr_step, 2*M_PI); + *init_carr_phase = + fmod(*init_carr_phase + num_samples * carr_step, 2*M_PI); } #else @@ -359,6 +460,12 @@ static void track_correlate(enum correlator_type correlator_type, code_L = l2c_cm_get_chip(code, code_phase + 0.5); code_phase_new = l2c_cm_get_code_phase(code_phase, code_step); break; + case GLOCA_CORRELATOR: + code_E = glo_ca_get_chip(code, code_phase - 0.5); + code_P = glo_ca_get_chip(code, code_phase); + code_L = glo_ca_get_chip(code, code_phase + 0.5); + code_phase_new = glo_ca_get_code_phase(code_phase, code_step); + break; default: break; } diff --git a/tests/check_correlator.c b/tests/check_correlator.c index e8dcd989..4f588196 100644 --- a/tests/check_correlator.c +++ b/tests/check_correlator.c @@ -4,12 +4,14 @@ #include #include -#define L1CA_CHIPS_PER_PRN_CODE 1023 -#define L2C_CM_CHIPS_PER_PRN_CODE 10230 +#define L1CA_CHIPS_PER_PRN_CODE 1023 +#define L2C_CM_CHIPS_PER_PRN_CODE 10230 +#define GLOCA_CHIPS_PER_PRN_CODE 511 enum signal_type { L1CA_SIGNAL, - L2C_SIGNAL + L2C_SIGNAL, + GLOCA_SIGNAL }; //#define DUMP_RESULTS @@ -207,6 +209,15 @@ static const u8 gps_l2cm_code[] = { 0xF1, 0x06, 0x28 }; +static const u8 glo_ca_code[] = { + 0xFE, 0x0F, 0x7C, 0x5C, 0xC8, 0x25, 0x3B, 0x47, 0x9F, 0x36, 0x2A, + 0x47, 0x1B, 0x57, 0x13, 0x11, 0x00, 0x84, 0x61, 0x39, 0x56, 0x1B, + 0xD3, 0x72, 0x28, 0x56, 0x9F, 0xB2, 0x4B, 0x7E, 0x4D, 0x4C, 0xC0, + 0x63, 0x28, 0xD2, 0xFE, 0x8B, 0x1D, 0x65, 0x9E, 0x3E, 0xE8, 0x35, + 0xB7, 0x60, 0xB5, 0xF5, 0x50, 0x29, 0x5E, 0x5D, 0xC0, 0xE7, 0x49, + 0xEB, 0xA8, 0x90, 0xCE, 0x17, 0xB6, 0x68, 0x77, 0x86 +}; + struct signal { s8* samples; size_t size; @@ -245,6 +256,7 @@ struct signal generate_signal(enum signal_type signal_type, double ip; switch (signal_type) { case L1CA_SIGNAL: + case GLOCA_SIGNAL: code = prn_code[(int)code_phase]; break; case L2C_SIGNAL: @@ -259,9 +271,9 @@ struct signal generate_signal(enum signal_type signal_type, static bool header = 1; static FILE *f; if (header) { - f = fopen("./gen.csv", "w"); - fprintf(f, "code_phase,prn\n"); - header = 0; + f = fopen("./gen.csv", "w"); + fprintf(f, "code_phase,prn\n"); + header = 0; } fprintf(f, "%f,%d\n", code_phase, code); @@ -288,6 +300,11 @@ struct signal generate_signal(enum signal_type signal_type, code_phase -= 2 * L2C_CM_CHIPS_PER_PRN_CODE; } break; + case GLOCA_SIGNAL: + if (code_phase >= GLOCA_CHIPS_PER_PRN_CODE) { + code_phase -= GLOCA_CHIPS_PER_PRN_CODE; + } + break; } } signal.samples = samples; @@ -323,11 +340,12 @@ static s8* get_prn_code(const u8 *code, size_t code_size, return prn_code; } -#define SAMPLING_FREQ_HZ 25e6 -#define L1CA_CHIPPING_RATE_HZ 1.023e6 -#define L2C_CM_CHIPPING_RATE_HZ 1.023e6 -#define IF_FREQUENCY_HZ 2e6 -#define CARRIER_DOPPLER_FREQ_HZ 100 +#define SAMPLING_FREQ_HZ 25e6 +#define L1CA_CHIPPING_RATE_HZ 1.023e6 +#define L2C_CM_CHIPPING_RATE_HZ 1.023e6 +#define GLOCA_CHIPPING_RATE_HZ 0.511e6 +#define IF_FREQUENCY_HZ 2e6 +#define CARRIER_DOPPLER_FREQ_HZ 100 START_TEST(test_l1ca_correlator) { @@ -363,7 +381,7 @@ START_TEST(test_l1ca_correlator) l1_ca_track_correlate(signal.samples, signal.size, code, &init_code_phase, - L1CA_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ, + (L1CA_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ / 1540.0) / SAMPLING_FREQ_HZ, &init_carr_phase, (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ, &I_E, &Q_E, @@ -424,7 +442,8 @@ START_TEST(test_l2c_cm_correlator) l2c_cm_track_correlate(signal.samples, signal.size, code, &init_code_phase, - L2C_CM_CHIPPING_RATE_HZ / SAMPLING_FREQ_HZ, + (L2C_CM_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ / 1200.0) /\ + SAMPLING_FREQ_HZ, &init_carr_phase, (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI / SAMPLING_FREQ_HZ, &I_E, &Q_E, @@ -451,6 +470,74 @@ START_TEST(test_l2c_cm_correlator) } END_TEST +#define GLOL1_CH0_CARRIER_FREQ_MHZ 1602.0 + +START_TEST(test_gloca_correlator) +{ + struct signal signal; + s8* code; + double init_code_phase = 0; + double init_carr_phase = 0; + double I_E; + double Q_E; + double I_P; + double Q_P; + double I_L; + double Q_L; + u32 num_samples; + double ca2co_scale = GLOCA_CHIPPING_RATE_HZ /\ + GLOL1_CH0_CARRIER_FREQ_MHZ; + + code = get_prn_code(glo_ca_code, sizeof(glo_ca_code), + GLOCA_CHIPS_PER_PRN_CODE); + fail_if(NULL == code, "Could not allocate PRN code data"); + + signal = generate_signal( GLOCA_SIGNAL, /* signal type */ + IF_FREQUENCY_HZ, /* intermediate freq */ + GLOCA_CHIPPING_RATE_HZ, /* code freq */ + CARRIER_DOPPLER_FREQ_HZ,/* carr dopp freq */ + ca2co_scale, /* carr to code scale factor */ + SAMPLING_FREQ_HZ, /* sampling frequency */ + code, /* PRN code data */ + 1); /* milliseconds to generate */ + + fail_if(NULL == signal.samples, "Could not generate signal data"); + + debug_start_timing(); + + glo_ca_track_correlate(signal.samples, signal.size, + code, + &init_code_phase, + (GLOCA_CHIPPING_RATE_HZ + CARRIER_DOPPLER_FREQ_HZ * ca2co_scale)\ + / SAMPLING_FREQ_HZ, + &init_carr_phase, + (IF_FREQUENCY_HZ + CARRIER_DOPPLER_FREQ_HZ) * 2.0 * M_PI\ + / SAMPLING_FREQ_HZ, + &I_E, &Q_E, + &I_P, &Q_P, + &I_L, &Q_L, + &num_samples); + + debug_stop_timing(); + + debug_dump_results(num_samples, I_E, Q_E, I_P, Q_P, I_L, Q_L); + + /* samples per PRN code */ + u32 expected_samples = SAMPLING_FREQ_HZ / 1000; + fail_if(num_samples != expected_samples); + + fail_if((I_P != 0) && (fabs(I_E / I_P) > 0.6)); + fail_if((I_P != 0) && (fabs(I_L / I_P) > 0.6)); + + fail_if((I_E != 0) && (fabs(Q_E / I_E) > 0.006)); + fail_if((I_P != 0) && (fabs(Q_P / I_P) > 0.005)); + fail_if((I_L != 0) && (fabs(Q_L / I_L) > 0.006)); + + free(code); + free(signal.samples); +} +END_TEST + Suite* correlator_suite(void) { Suite *s = suite_create("Correlator"); @@ -458,6 +545,7 @@ Suite* correlator_suite(void) tcase_add_test(tc_core, test_l1ca_correlator); tcase_add_test(tc_core, test_l2c_cm_correlator); + tcase_add_test(tc_core, test_gloca_correlator); suite_add_tcase(s, tc_core); return s;