From ce1d80b476bce15593058e795172f49b4e4c2429 Mon Sep 17 00:00:00 2001 From: Roman Gezikov Date: Wed, 8 Jun 2016 12:34:14 +0300 Subject: [PATCH 01/21] Change solution to count satellites, not tracking channels --- src/Makefile | 1 + src/sid_set.c | 63 ++++++++++++++++++++++++++++++++++++++++++++++++++ src/sid_set.h | 27 ++++++++++++++++++++++ src/solution.c | 16 +++++++++++-- 4 files changed, 105 insertions(+), 2 deletions(-) create mode 100644 src/sid_set.c create mode 100644 src/sid_set.h diff --git a/src/Makefile b/src/Makefile index 389c5187..cc7ce3a4 100644 --- a/src/Makefile +++ b/src/Makefile @@ -113,6 +113,7 @@ CSRC := $(STARTUPSRC) \ $(SWIFTNAV_ROOT)/src/decode.o \ $(SWIFTNAV_ROOT)/src/signal.o \ $(SWIFTNAV_ROOT)/src/l2c_capb.o \ + $(SWIFTNAV_ROOT)/src/sid_set.o \ main.c # C++ sources that can be compiled in ARM or THUMB mode depending on the global diff --git a/src/sid_set.c b/src/sid_set.c new file mode 100644 index 00000000..9e6c964a --- /dev/null +++ b/src/sid_set.c @@ -0,0 +1,63 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * 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. + */ + +#include +#include +#include +#include "sid_set.h" + +/** Initialize new sid set + * + * \param sid_list gnss_sid_set_t to be initialized + * + */ +void sid_set_init(gnss_sid_set_t *sid_set) +{ + memset(sid_set->sats, 0, sizeof(sid_set->sats)); +} + +/** Add new element to the set + * + * \param sid_set gnss_sid_set_t to add new element to + * \param sid element to be added + * + */ +void sid_set_add(gnss_sid_set_t *sid_set, const gnss_signal_t sid) +{ + u16 s = sid_to_code_index(sid); + assert(s < 64); + sid_set->sats[sid.code] |= ((u64)0x01 << s); +} + +/** Get number of satellites in sid set + * + * \param sid_set gnss_sid_set_t to count number of satellites in + * + * \returns Number of unique satellites present in the set. + * + */ +u32 sid_set_get_sat_count(const gnss_sid_set_t *sid_set) +{ + u64 sats[CONSTELLATION_COUNT]; + memset(sats, 0, sizeof(sats)); + for (enum code code = 0; code < CODE_COUNT; code++) { + sats[code_to_constellation(code)] |= sid_set->sats[code]; + } + + u32 cnt = 0; + for (enum constellation constellation = 0; + constellation < CONSTELLATION_COUNT; constellation++) { + cnt += count_bits_u64(sats[constellation], 1); + } + + return cnt; +} diff --git a/src/sid_set.h b/src/sid_set.h new file mode 100644 index 00000000..bb50d9d8 --- /dev/null +++ b/src/sid_set.h @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * 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. + */ + +#ifndef SRC_SID_SET_H_ +#define SRC_SID_SET_H_ + +#include +#include + +typedef struct { + u64 sats[CODE_COUNT]; +} gnss_sid_set_t; + +void sid_set_init(gnss_sid_set_t *sid_set); +void sid_set_add(gnss_sid_set_t *sid_set, const gnss_signal_t sid); +u32 sid_set_get_sat_count(const gnss_sid_set_t *sid_set); + +#endif /* SRC_SID_SET_H_ */ diff --git a/src/solution.c b/src/solution.c index 9597bb1e..b618c553 100644 --- a/src/solution.c +++ b/src/solution.c @@ -43,6 +43,7 @@ #include "signal.h" #include "system_monitor.h" #include "main.h" +#include "sid_set.h" /* Maximum CPU time the solution thread is allowed to use. */ #define SOLN_THD_CPU_MAX (0.60f) @@ -429,7 +430,12 @@ static void solution_thread(void *arg) tracking_channel_unlock(i); } - if (n_ready < 4) { + gnss_sid_set_t codes_in_track; + sid_set_init(&codes_in_track); + for (u8 i=0; i Date: Wed, 6 Apr 2016 18:02:25 +0300 Subject: [PATCH 02/21] Change cutoff frequency --- src/track/track_gps_l1ca.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index b58c0ca7..eece1168 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -48,7 +48,7 @@ #define LD_PARAMS_EXTRAOPT "0.02, 0.8, 150, 50" #define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" -#define CN0_EST_LPF_CUTOFF 5 +#define CN0_EST_LPF_CUTOFF 0.1 static struct loop_params { float code_bw, code_zeta, code_k, carr_to_code; From c89d1c55cc70eeda724c38a7f6244a2f6ea78acd Mon Sep 17 00:00:00 2001 From: Valeri Atamaniouk Date: Tue, 31 May 2016 13:17:52 +0300 Subject: [PATCH 03/21] cn0_est: memory optimization Optimized memory used by C/N0 filter to increase platform stability. --- src/track/track_gps_l1ca.c | 57 ++++++++++++++++++++++++++++++++++++-- 1 file changed, 55 insertions(+), 2 deletions(-) diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index eece1168..a83c44d7 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -48,7 +48,7 @@ #define LD_PARAMS_EXTRAOPT "0.02, 0.8, 150, 50" #define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" -#define CN0_EST_LPF_CUTOFF 0.1 +#define CN0_EST_LPF_CUTOFF 0.1f static struct loop_params { float code_bw, code_zeta, code_k, carr_to_code; @@ -238,7 +238,60 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, corr_t* cs = data->cs; /* Update C/N0 estimate */ - common_data->cn0 = cn0_est(&data->cn0_est, cs[1].I/data->int_ms, cs[1].Q/data->int_ms); + { + /* Pre-computed C/N0 estimator and filter parameters. The parameters are + * computed using equivalent of cn0_est_compute_params() function for + * integration periods of 1, 2, 4, 5, 10 and 20ms and cut-off frequency + * of 0.1 Hz. + */ + static const cn0_est_params_t pre_computed[] = { + {3.0000000e+01f, 9.8652211e-08f, -1.9991114e+00f, 9.9911182e-01f}, + {2.6989700e+01f, 3.9443364e-07f, -1.9982228e+00f, 9.9822443e-01f}, + {2.3979400e+01f, 1.5763345e-06f, -1.9964457e+00f, 9.9645200e-01f}, + {2.3010300e+01f, 2.4619300e-06f, -1.9955571e+00f, 9.9556697e-01f}, + {2.0000000e+01f, 9.8259168e-06f, -1.9911143e+00f, 9.9115360e-01f}, + {1.6989700e+01f, 3.9130205e-05f, -1.9822289e+00f, 9.8238545e-01f} + }; + + cn0_est_params_t params; + const cn0_est_params_t *pparams = NULL; + + switch (data->int_ms) { + case 1: + pparams = &pre_computed[0]; + break; + + case 2: + pparams = &pre_computed[1]; + break; + + case 4: + pparams = &pre_computed[2]; + break; + + case 5: + pparams = &pre_computed[3]; + break; + + case 10: + pparams = &pre_computed[4]; + break; + + case 20: + pparams = &pre_computed[5]; + break; + + default: + cn0_est_compute_params(¶ms, 1e3f / data->int_ms, CN0_EST_LPF_CUTOFF, + 1e3f / data->int_ms); + pparams = ¶ms; + break; + } + common_data->cn0 = cn0_est(&data->cn0_est, + pparams, + cs[1].I/data->int_ms, cs[1].Q/data->int_ms); + } + if (common_data->cn0 > track_cn0_drop_thres) common_data->cn0_above_drop_thres_count = common_data->update_count; From c1dcef9dda5873df93b385d765497178a6e42527 Mon Sep 17 00:00:00 2001 From: Tommi Paakki Date: Wed, 1 Jun 2016 11:57:01 +0300 Subject: [PATCH 04/21] Increase cn0_use_threshold --- src/track/track_gps_l1ca.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index a83c44d7..77a9e8d1 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -61,7 +61,7 @@ static struct lock_detect_params { u16 lp, lo; } lock_detect_params; -static float track_cn0_use_thres = 31.0; /* dBHz */ +static float track_cn0_use_thres = 37.0; /* dBHz */ static float track_cn0_drop_thres = 31.0; static char loop_params_string[120] = LOOP_PARAMS_MED; From 945c69bb06021fdab6ddd8712f4b259b2e13b562 Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Wed, 11 May 2016 13:49:09 +0300 Subject: [PATCH 05/21] Add L1CA to L2C handover --- src/board/v3/Makefile.inc | 1 + src/manage.h | 1 + src/track/track_gps_l2cm.c | 133 +++++++++++++++++++++++++++++++++++++ src/track/track_gps_l2cm.h | 23 +++++++ 4 files changed, 158 insertions(+) create mode 100644 src/track/track_gps_l2cm.c create mode 100644 src/track/track_gps_l2cm.h diff --git a/src/board/v3/Makefile.inc b/src/board/v3/Makefile.inc index 9a669640..495aa9d7 100644 --- a/src/board/v3/Makefile.inc +++ b/src/board/v3/Makefile.inc @@ -77,6 +77,7 @@ BOARDSRC := \ $(BOARDDIR)/nap/track_channel.o \ $(BOARDDIR)/platform_signal.o \ $(SWIFTNAV_ROOT)/src/track/track_gps_l1ca.o \ + $(SWIFTNAV_ROOT)/src/track/track_gps_l2cm.o \ $(SWIFTNAV_ROOT)/src/decode/decode_gps_l1ca.o \ BOARDASM := \ diff --git a/src/manage.h b/src/manage.h index 499cea21..9e42f294 100644 --- a/src/manage.h +++ b/src/manage.h @@ -67,6 +67,7 @@ typedef struct { u32 sample_count; /**< Reference NAP sample count. */ float carrier_freq; /**< Carrier frequency Doppler (Hz). */ float code_phase; /**< Code phase (chips). */ + u32 chips_to_correlate; /**< Chips to integrate over. */ float cn0_init; /**< C/N0 estimate (dBHz). */ s8 elevation; /**< Elevation (deg). */ } tracking_startup_params_t; diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c new file mode 100644 index 00000000..eb3ea059 --- /dev/null +++ b/src/track/track_gps_l2cm.c @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Adel Mamin + * Pasi Miettinen + * + * 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. + */ + +#include "board.h" +#include "nap/nap_common.h" +#include "nap/track_channel.h" +#include "track.h" +#include "main.h" + +#include "track.h" + +#include "track_gps_l2cm.h" +#include "track_api.h" +#include "decode.h" +#include "manage.h" + +#include +#include +#include +#include + +#include +#include +#include "math.h" + +#include "settings.h" +#include "signal.h" + +/* Number of chips to integrate over in the short cycle interval [chips] + * The value must be within [0..(2 * GPS_L2CM_CHIPS_NUM)]. + */ +#define L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS 1023 + +/** Do L1C/A to L2 CM handover. + * + * The condition for the handover is the availability of bitsync on L1 C/A + * + * \param sample_count NAP sample count + * \param sat L1C/A Satellite ID + * \param nap_channel Associated NAP channel + * \param code_phase L1CA code phase [chips] + */ +void do_l1ca_to_l2cm_handover(u32 sample_count, + u16 sat, + u8 nap_channel, + float code_phase) +{ + /* First, get L2C capability for the SV from NDB */ + u32 l2c_cpbl; + // TODO: uncomment this as soon as NDB gets available + // ndb_gps_l2cm_l2c_cap_read(&l2c_cpbl); + + // TODO: remove this as soon as NDB gets available + // GPS PRNs with L2C capability: + // 01 03 05 06 07 08 09 10 12 15 17 24 25 26 27 29 30 31 32 + // as per http://tinyurl.com/zj5q62h + l2c_cpbl = (u32)1 << (1 - 1); + l2c_cpbl |= (u32)1 << (3 - 1); + l2c_cpbl |= (u32)1 << (5 - 1); + l2c_cpbl |= (u32)1 << (6 - 1); + l2c_cpbl |= (u32)1 << (7 - 1); + l2c_cpbl |= (u32)1 << (8 - 1); + l2c_cpbl |= (u32)1 << (9 - 1); + l2c_cpbl |= (u32)1 << (10 - 1); + l2c_cpbl |= (u32)1 << (12 - 1); + l2c_cpbl |= (u32)1 << (15 - 1); + l2c_cpbl |= (u32)1 << (17 - 1); + l2c_cpbl |= (u32)1 << (24 - 1); + l2c_cpbl |= (u32)1 << (25 - 1); + l2c_cpbl |= (u32)1 << (26 - 1); + l2c_cpbl |= (u32)1 << (27 - 1); + l2c_cpbl |= (u32)1 << (29 - 1); + l2c_cpbl |= (u32)1 << (30 - 1); + l2c_cpbl |= (u32)1 << (31 - 1); + l2c_cpbl |= (u32)1 << (32 - 1); + + /* compose SID: same SV, but code is L2 CM */ + gnss_signal_t sid = { + .sat = sat, + .code = CODE_GPS_L2CM + }; + + if (0 == (l2c_cpbl & ((u32)1 << (sat - 1)))) { + log_info_sid(sid, "SV does not support L2C signal"); + return; + } + + if ((code_phase < 0) || + ((code_phase > 0.5) && (code_phase < (GPS_L1CA_CHIPS_NUM - 0.5)))) { + log_warn_sid(sid, "Unexpected L1C/A to L2C handover code phase: %f", + code_phase); + return; + } + + if (code_phase > (GPS_L1CA_CHIPS_NUM - 0.5)) { + code_phase = 2 * GPS_L2CM_CHIPS_NUM - (GPS_L1CA_CHIPS_NUM - code_phase); + } + + /* recalculate doppler freq for L2 from L1*/ + double carrier_freq = tracking_channel_carrier_freq_get(nap_channel) * + GPS_L2_HZ / GPS_L1_HZ; + + /* get initial cn0 from parent L1 channel */ + float cn0_init = tracking_channel_cn0_get(nap_channel); + + s8 elevation = tracking_channel_evelation_degrees_get(nap_channel); + + tracking_startup_params_t startup_params = { + .sid = sid, + .sample_count = sample_count, + .carrier_freq = carrier_freq, + .code_phase = code_phase, + .chips_to_correlate = L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS, + .cn0_init = cn0_init, + .elevation = elevation + }; + + if (tracking_startup_request(&startup_params)) { + log_info_sid(sid, "L2 CM handover done"); + } else { + log_error_sid(sid, "Failed to start L2C tracking"); + } +} diff --git a/src/track/track_gps_l2cm.h b/src/track/track_gps_l2cm.h new file mode 100644 index 00000000..431784b2 --- /dev/null +++ b/src/track/track_gps_l2cm.h @@ -0,0 +1,23 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Jacob McNamee + * 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. + */ +#ifndef SWIFTNAV_TRACK_GPS_L2CM_H +#define SWIFTNAV_TRACK_GPS_L2CM_H + +#include + +void do_l1ca_to_l2cm_handover(u32 sample_count, + u16 sat, + u8 nap_channel, + float code_phase); + +#endif /* SWIFTNAV_TRACK_GPS_L2CM_H */ From c984974f6a7a4087d40c03c616ae7f1c76b3e8fe Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Thu, 12 May 2016 14:35:14 +0300 Subject: [PATCH 06/21] Add L2C tracking --- libswiftnav | 2 +- src/board/nap/track_channel.h | 6 +- src/board/v2/nap/track_channel.c | 18 +- src/board/v3/nap/nap_common.c | 6 +- src/board/v3/nap/nap_constants.h | 9 +- src/board/v3/nap/nap_hw.h | 25 ++ src/board/v3/nap/track_channel.c | 159 +++++++++- src/board/v3/platform_signal.c | 2 + src/board/v3/platform_signal.h | 2 +- src/manage.c | 16 +- src/manage.h | 2 +- src/sbp_utils.c | 2 +- src/simulator.c | 3 +- src/track.c | 28 +- src/track.h | 7 +- src/track/track_gps_l1ca.c | 21 +- src/track/track_gps_l2cm.c | 507 ++++++++++++++++++++++++++++++- src/track/track_gps_l2cm.h | 12 +- src/track_api.c | 15 +- src/track_api.h | 2 +- 20 files changed, 792 insertions(+), 52 deletions(-) diff --git a/libswiftnav b/libswiftnav index f82b1f42..51bafb45 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit f82b1f42224dd9cd8dabd037907c731f1b94a867 +Subproject commit 51bafb452a09bf497b1a40f331642b4cafc8ef6f diff --git a/src/board/nap/track_channel.h b/src/board/nap/track_channel.h index c6c6430b..c4d10b54 100644 --- a/src/board/nap/track_channel.h +++ b/src/board/nap/track_channel.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2014 Swift Navigation Inc. + * Copyright (C) 2011-2014,2016 Swift Navigation Inc. * Contact: Fergus Noble * Colin Beighley * @@ -27,10 +27,10 @@ extern u8 nap_track_n_channels; /** \} */ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, - float carrier_freq, float code_phase); + float carrier_freq, float code_phase, u32 chips_to_correlate); void nap_track_update(u8 channel, double carrier_freq, - double code_phase_rate, u8 rollover_count, + double code_phase_rate, u32 chips_to_correlate, u8 corr_spacing); void nap_track_read_results(u8 channel, u32* count_snapshot, corr_t corrs[], diff --git a/src/board/v2/nap/track_channel.c b/src/board/v2/nap/track_channel.c index c388d990..20fc5f5b 100644 --- a/src/board/v2/nap/track_channel.c +++ b/src/board/v2/nap/track_channel.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2014 Swift Navigation Inc. + * Copyright (C) 2011-2014,2016 Swift Navigation Inc. * Contact: Fergus Noble * Colin Beighley * @@ -81,16 +81,19 @@ static struct nap_ch_state { * \param prn CA code PRN (0-31) to track. (deprecated) * \param carrier_phase Initial code phase. * \param code_phase Initial carrier phase. + * \param chips_to_correlate How many chips to correlate (unused) */ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, - float carrier_freq, float code_phase) + float carrier_freq, float code_phase, u32 chips_to_correlate) { struct nap_ch_state *s = &nap_ch_state[channel]; memset(s, 0, sizeof(*s)); + (void) chips_to_correlate; + u32 track_count = nap_timing_count() + 20000; float cp = propagate_code_phase(code_phase, carrier_freq, - track_count - ref_timing_count); + track_count - ref_timing_count, sid.code); /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ @@ -133,17 +136,24 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, * \param channel NAP track channel whose UPDATE register to write. * \param carrier_freq Next correlation period's carrier frequency. * \param code_phase_rate Next correlation period's code phase rate. + * \param chips_to_correlate How many chips to correlate over. + * \param corr_spacing Spacing between correlator's EPL replicas. */ void nap_track_update(u8 channel, double carrier_freq, - double code_phase_rate, u8 rollover_count, + double code_phase_rate, u32 chips_to_correlate, u8 corr_spacing) { + u8 rollover_count; struct nap_ch_state *s = &nap_ch_state[channel]; s->carr_pinc_prev = s->carr_pinc; s->carr_pinc = carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ; s->code_pinc_prev = s->code_pinc; s->code_pinc = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; + rollover_count = (u8) (chips_to_correlate / GPS_L1CA_CHIPS_NUM); + if (rollover_count) { + rollover_count -= 1; + } nap_track_update_wr_blocking(channel, s->carr_pinc, s->code_pinc, rollover_count, corr_spacing); } diff --git a/src/board/v3/nap/nap_common.c b/src/board/v3/nap/nap_common.c index 6e55e2cb..5526ebe7 100644 --- a/src/board/v3/nap/nap_common.c +++ b/src/board/v3/nap/nap_common.c @@ -47,7 +47,11 @@ void nap_setup(void) axi_dma_init(); axi_dma_start(&AXIDMADriver1); - NAP->FE_PINC[0] = NAP_FE_BASEBAND_MIXER_PINC; + /* NAP_FE10_PINC initialization for GPS L1C/A processing */ + NAP->FE_PINC[0] = NAP_FE_L1CA_BASEBAND_MIXER_PINC; + + /* NAP_FE40_PINC initialization for GPS L2C processing */ + NAP->FE_PINC[6] = NAP_FE_L2C_BASEBAND_MIXER_PINC; /* Enable NAP interrupt */ chThdCreateStatic(wa_nap_exti, sizeof(wa_nap_exti), HIGHPRIO-1, nap_exti_thread, NULL); diff --git a/src/board/v3/nap/nap_constants.h b/src/board/v3/nap/nap_constants.h index 13bb5f1a..f848b384 100644 --- a/src/board/v3/nap/nap_constants.h +++ b/src/board/v3/nap/nap_constants.h @@ -44,7 +44,14 @@ * @brief The phase increment used to mix the frontend samples to baseband * @note 4294967296 is 2^32 and the .5 is for rounding */ -#define NAP_FE_BASEBAND_MIXER_PINC (u32)(14.58e6 * 4294967296.0 \ +#define NAP_FE_L1CA_BASEBAND_MIXER_PINC (u32)(14.58e6 * 4294967296.0 \ + / NAP_FRONTEND_SAMPLE_RATE_Hz + 0.5) + +/** + * @brief The phase increment used to mix the frontend samples to baseband + * @note 4294967296 is 2^32 and the .5 is for rounding + */ +#define NAP_FE_L2C_BASEBAND_MIXER_PINC (u32)(7.4e6 * 4294967296.0 \ / NAP_FRONTEND_SAMPLE_RATE_Hz + 0.5) #define NAP_KEY_LENGTH (16) diff --git a/src/board/v3/nap/nap_hw.h b/src/board/v3/nap/nap_hw.h index dd24d6be..d3368d2a 100644 --- a/src/board/v3/nap/nap_hw.h +++ b/src/board/v3/nap/nap_hw.h @@ -116,6 +116,31 @@ typedef struct { #define NAP_TRK_SPACING_INNER_Pos (16U) #define NAP_TRK_SPACING_INNER_Msk (0xFFFFU << NAP_TRK_SPACING_INNER_Pos) +#define NAP_TRK_CONTROL_CODE_Pos (9U) +#define NAP_TRK_CONTROL_CODE_Msk (0x3U << NAP_TRK_CONTROL_CODE_Pos) + +#define NAP_TRK_CONTROL_RF_FE_Pos (1U) +#define NAP_TRK_CONTROL_RF_FE_Msk (0x3U << NAP_TRK_CONTROL_RF_FE_Pos) + +#define NAP_TRK_CONTROL_RF_FE_CH_Pos (0U) +#define NAP_TRK_CONTROL_RF_FE_CH_Msk (0x1U << NAP_TRK_CONTROL_RF_FE_CH_Pos) + +#define NAP_TRK_STATUS_RUNNING (1 << 31) + +/* NAP RF fronend channel ID */ +enum { + NAP_RF_FRONTEND_CHANNEL_1 = 0, /* GPS L1 */ + NAP_RF_FRONTEND_CHANNEL_2, /* Glonass L1 */ + NAP_RF_FRONTEND_CHANNEL_3, /* Glonass L2 */ + NAP_RF_FRONTEND_CHANNEL_4 /* GPS L2 */ +}; + +/* The values of NAP_TRKx_CONTROL::CODE field */ +enum { + NAP_CONSTELLATION_BAND_1 = 0, /* GPS L1C/A, SBAS L1CA */ + NAP_CONSTELLATION_BAND_2 /* GPS L2CM */ +}; + /* Instances */ #define NAP ((nap_t *)0x43C00000) diff --git a/src/board/v3/nap/track_channel.c b/src/board/v3/nap/track_channel.c index f6499f13..6c309b7c 100644 --- a/src/board/v3/nap/track_channel.c +++ b/src/board/v3/nap/track_channel.c @@ -54,40 +54,170 @@ static struct nap_ch_state { u32 code_phase; /**< Fractional part of code phase. */ } nap_ch_state[NAP_MAX_N_TRACK_CHANNELS]; -static u32 calc_length_samples(u8 codes, u32 cp_start_frac_units, +/** Compute the correlation length in the units of sampling frequency samples. + * \param chips_to_correlate The number of chips to correlate over. + * \param cp_start_frac_units Initial code phase in NAP units. + * \param cp_rate_units Code phase rate. + * \return The correlation length in NAP units + */ +static u32 calc_length_samples(u32 chips_to_correlate, u32 cp_start_frac_units, u32 cp_rate_units) { - u64 cp_end_units = codes * 1023 * NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP; + u64 cp_end_units = chips_to_correlate * NAP_TRACK_CODE_PHASE_UNITS_PER_CHIP; /* cp_start_frac_units is reinterpreted as a signed value. This works * because NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH is equal to 32 */ u64 cp_units = cp_end_units - (s32)cp_start_frac_units; u32 samples = cp_units / cp_rate_units; + return samples; } +/** Return the init value of NAP G1 pseudorandom generator. + * \param sid Signal ID + * \return NAP G1 initial value + */ +u32 sid_to_init_g1(gnss_signal_t sid) +{ + u32 ret = ~0; + /* The L2C G1 init values are taken from IS-GPS-200H Table 3-IIa + "Code phase assignments (IIR-M, IIF, and subsequent blocks only)" + For example, PRN 1 has G1 init value 0742417664. + */ + u32 gps_l2cm_prns_init_values[] = { + /* 0 */ 0742417664, + /* 1 */ 0756014035, + /* 2 */ 0002747144, + /* 3 */ 0066265724, + /* 4 */ 0601403471, + /* 5 */ 0703232733, + /* 6 */ 0124510070, + /* 7 */ 0617316361, + /* 8 */ 0047541621, + /* 9 */ 0733031046, + /* 10 */ 0713512145, + /* 11 */ 0024437606, + /* 12 */ 0021264003, + /* 13 */ 0230655351, + /* 14 */ 0001314400, + /* 15 */ 0222021506, + /* 16 */ 0540264026, + /* 17 */ 0205521705, + /* 18 */ 0064022144, + /* 19 */ 0120161274, + /* 20 */ 0044023533, + /* 21 */ 0724744327, + /* 22 */ 0045743577, + /* 23 */ 0741201660, + /* 24 */ 0700274134, + /* 25 */ 0010247261, + /* 26 */ 0713433445, + /* 27 */ 0737324162, + /* 28 */ 0311627434, + /* 29 */ 0710452007, + /* 30 */ 0722462133, + /* 31 */ 0050172213, + /* 32 */ 0500653703, + /* 33 */ 0755077436, + /* 34 */ 0136717361, + /* 35 */ 0756675453, + /* 36 */ 0435506112 + }; + + switch (sid.code) { + case CODE_GPS_L2CM: + ret = gps_l2cm_prns_init_values[sid.sat - 1] & 0x7FFFFFF; + break; + case CODE_GPS_L1CA: + ret = 0x3ff; + break; + default: + assert(0); + break; + } + + return ret; +} + +/** Looks-up RF frontend channel for the given signal ID. + * \param sid Signal ID. + * \return RF front-end channel number. + */ +u8 sid_to_rf_frontend_channel(gnss_signal_t sid) +{ + u8 ret = ~0; + switch (sid.code) { + case CODE_GPS_L1CA: + case CODE_SBAS_L1CA: + ret = NAP_RF_FRONTEND_CHANNEL_1; + break; + case CODE_GPS_L2CM: + ret = NAP_RF_FRONTEND_CHANNEL_4; + break; + default: + assert(0); + break; + } + return ret; +} + +/** Looks-up NAP constellation and band code for the given signal ID. + * \param sid Signal ID. + * \return NAP constallation and band code. + */ +u8 sid_to_nap_code(gnss_signal_t sid) +{ + u8 ret = ~0; + switch (sid.code) { + case CODE_GPS_L1CA: + case CODE_SBAS_L1CA: + ret = NAP_CONSTELLATION_BAND_1; + break; + case CODE_GPS_L2CM: + ret = NAP_CONSTELLATION_BAND_2; + break; + default: + assert(0); + break; + } + return ret; +} + void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, - float carrier_freq, float code_phase) + float carrier_freq, float code_phase, u32 chips_to_correlate) { - assert(sid.code == CODE_GPS_L1CA); + assert((sid.code == CODE_GPS_L1CA) || (sid.code == CODE_GPS_L2CM)); nap_trk_regs_t *t = &NAP->TRK_CH[channel]; struct nap_ch_state *s = &nap_ch_state[channel]; memset(s, 0, sizeof(*s)); + u16 control; u8 prn = sid.sat - GPS_FIRST_PRN; - t->CONTROL = prn << NAP_TRK_CONTROL_SAT_Pos; - + /* PRN code */ + control = (prn << NAP_TRK_CONTROL_SAT_Pos) & NAP_TRK_CONTROL_SAT_Msk; + /* RF frontend channel */ + control |= (sid_to_rf_frontend_channel(sid) << NAP_TRK_CONTROL_RF_FE_Pos) & \ + NAP_TRK_CONTROL_RF_FE_Msk; + /* Constellation and band for tracking */ + control |= (sid_to_nap_code(sid) << NAP_TRK_CONTROL_CODE_Pos) & \ + NAP_TRK_CONTROL_CODE_Msk; + /* We are not utilizing multiple signals within one RF channel at the moment. + Therefore, RF_FE_CH is 0 and below statement in a NOP. */ + control |= (0 << NAP_TRK_CONTROL_RF_FE_CH_Pos) & NAP_TRK_CONTROL_RF_FE_CH_Msk; + + t->CONTROL = control; /* We always start at zero code phase */ t->CODE_INIT_INT = 0; t->CODE_INIT_FRAC = 0; - t->CODE_INIT_G1 = 0x3ff; + t->CODE_INIT_G1 = sid_to_init_g1(sid); t->CODE_INIT_G2 = 0x3ff; t->SPACING = (SPACING_HALF_CHIP << NAP_TRK_SPACING_OUTER_Pos) | (SPACING_HALF_CHIP << NAP_TRK_SPACING_INNER_Pos); - double cp_rate = (1.0 + carrier_freq / GPS_L1_HZ) * GPS_CA_CHIPPING_RATE; - nap_track_update(channel, carrier_freq, cp_rate, 0, 0); + double cp_rate = (1.0 + carrier_freq / code_to_carr_freq(sid.code)) * + code_to_chip_rate(sid.code); + nap_track_update(channel, carrier_freq, cp_rate, chips_to_correlate, 0); /* First integration is one sample short */ t->LENGTH += 1; @@ -105,11 +235,12 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, chSysLock(); tc_req = NAP->TIMING_COUNT + TIMING_COMPARE_DELTA_MIN; double cp = propagate_code_phase(code_phase, carrier_freq, - tc_req - ref_timing_count); + tc_req - ref_timing_count, sid.code); /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ - tc_req += (NAP_FRONTEND_SAMPLE_RATE_Hz / GPS_CA_CHIPPING_RATE) * (1023.0 - cp) * - (1.0 + carrier_freq / GPS_L1_HZ); + tc_req += (NAP_FRONTEND_SAMPLE_RATE_Hz / code_to_chip_rate(sid.code)) * + (code_to_chip_num(sid.code) - cp) * + (1.0 + carrier_freq / code_to_carr_freq(sid.code)); NAP->TRK_TIMING_COMPARE = tc_req; chSysUnlock(); @@ -130,7 +261,7 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, } void nap_track_update(u8 channel, double carrier_freq, - double code_phase_rate, u8 rollover_count, + double code_phase_rate, u32 chips_to_correlate, u8 corr_spacing) { (void)corr_spacing; /* This is always written as 0 now... */ @@ -141,7 +272,7 @@ void nap_track_update(u8 channel, double carrier_freq, u32 cp_rate_units = code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; t->CARR_PINC = -carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ; t->CODE_PINC = cp_rate_units; - t->LENGTH = calc_length_samples(rollover_count + 1, + t->LENGTH = calc_length_samples(chips_to_correlate, s->code_phase, cp_rate_units); } diff --git a/src/board/v3/platform_signal.c b/src/board/v3/platform_signal.c index 771f54d5..9b474436 100644 --- a/src/board/v3/platform_signal.c +++ b/src/board/v3/platform_signal.c @@ -13,12 +13,14 @@ #include "platform_signal.h" #include "track/track_gps_l1ca.h" +#include "track/track_gps_l2cm.h" #include "decode/decode_gps_l1ca.h" void platform_track_setup(void) { track_gps_l1ca_register(); + track_gps_l2cm_register(); } void platform_decode_setup(void) diff --git a/src/board/v3/platform_signal.h b/src/board/v3/platform_signal.h index 1932c94e..8da7a6fa 100644 --- a/src/board/v3/platform_signal.h +++ b/src/board/v3/platform_signal.h @@ -15,7 +15,7 @@ /* Platform-specific code support */ #define CODE_GPS_L1CA_SUPPORT 1 -#define CODE_GPS_L2CM_SUPPORT 0 +#define CODE_GPS_L2CM_SUPPORT 1 #define CODE_SBAS_L1CA_SUPPORT 1 #define CODE_GLO_L1CA_SUPPORT 0 #define CODE_GLO_L2CA_SUPPORT 0 diff --git a/src/manage.c b/src/manage.c index 6f2646cb..1f571cc4 100644 --- a/src/manage.c +++ b/src/manage.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2011-2014 Swift Navigation Inc. + * Copyright (C) 2011-2014,2016 Swift Navigation Inc. * Contact: Fergus Noble * * This source is subject to the license found in the file 'LICENSE' which must @@ -25,6 +25,7 @@ #include #include #include +#include #include "main.h" #include "board/nap/track_channel.h" @@ -192,6 +193,13 @@ void manage_acq_setup() track_mask[i] = false; almanac[i].valid = 0; + if (CODE_GPS_L2CM == acq_status[i].sid.code) { + /* Do not acquire GPS L2C. + * Do GPS L1 C/A to L2C handover at the tracking stage instead. */ + acq_status[i].state = ACQ_PRN_SKIP; + acq_status[i].masked = true; + } + if (!sbas_enabled && (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) { acq_status[i].masked = true; @@ -408,6 +416,7 @@ static void manage_acq() .sample_count = acq_result.sample_count, .carrier_freq = acq_result.cf, .code_phase = acq_result.cp, + .chips_to_correlate = GPS_L1CA_CHIPS_NUM, .cn0_init = acq_result.cn0, .elevation = TRACKING_ELEVATION_UNKNOWN }; @@ -448,7 +457,9 @@ static u8 manage_track_new_acq(gnss_signal_t sid) */ for (u8 i=0; i * * This source is subject to the license found in the file 'LICENSE' which must diff --git a/src/sbp_utils.c b/src/sbp_utils.c index 9a0be85b..305cc7f4 100644 --- a/src/sbp_utils.c +++ b/src/sbp_utils.c @@ -294,7 +294,7 @@ void unpack_ephemeris(const msg_ephemeris_t *msg, ephemeris_t *e) e->sid = sid_from_sbp(msg->sid); e->kepler.iode = msg->iode; e->kepler.iodc = msg->iodc; - e->fit_interval = 4 * 60 * 60; /* TODO: this is a work around until SBP updated */ + e->fit_interval = (u8)(4 * 60 * 60); /* TODO: this is a work around until SBP updated */ e->ura = 2.0f; /* TODO: this is a work around until SBP updated*/ } diff --git a/src/simulator.c b/src/simulator.c index 1fb3d67b..bfa3a98d 100644 --- a/src/simulator.c +++ b/src/simulator.c @@ -352,7 +352,8 @@ void populate_nav_meas(navigation_measurement_t *nav_meas, double dist, double e nav_meas->raw_pseudorange += rand_gaussian(sim_settings.pseudorange_sigma * sim_settings.pseudorange_sigma); - nav_meas->raw_carrier_phase = dist / (GPS_C / GPS_L1_HZ); + nav_meas->raw_carrier_phase = dist / (GPS_C / + code_to_carr_freq(simulation_almanacs[almanac_i].sid.code)); nav_meas->raw_carrier_phase += simulation_fake_carrier_bias[almanac_i]; nav_meas->raw_carrier_phase += rand_gaussian(sim_settings.phase_sigma * sim_settings.phase_sigma); diff --git a/src/track.c b/src/track.c index f4f43cdc..a94175bd 100644 --- a/src/track.c +++ b/src/track.c @@ -129,7 +129,8 @@ static void interface_function(tracker_channel_t *tracker_channel, tracker_interface_function_t func); static void event(tracker_channel_t *d, event_t event); static void common_data_init(tracker_common_data_t *common_data, - u32 sample_count, float carrier_freq, float cn0); + u32 sample_count, float carrier_freq, + float cn0, code_t code); static void tracker_channel_lock(tracker_channel_t *tracker_channel); static void tracker_channel_unlock(tracker_channel_t *tracker_channel); static void error_flags_clear(tracker_channel_t *tracker_channel); @@ -285,13 +286,15 @@ bool tracker_channel_available(tracker_channel_id_t id, gnss_signal_t sid) * * \return The propagated code phase in chips. */ -double propagate_code_phase(double code_phase, double carrier_freq, u32 n_samples) +double propagate_code_phase(double code_phase, double carrier_freq, + u32 n_samples, code_t code) { /* Calculate the code phase rate with carrier aiding. */ - double code_phase_rate = (1.0 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE; + double code_phase_rate = (1.0 + carrier_freq / code_to_carr_freq(code)) * + code_to_chip_rate(code); code_phase += n_samples * code_phase_rate / NAP_FRONTEND_SAMPLE_RATE_Hz; u32 cp_int = floor(code_phase); - code_phase -= cp_int - (cp_int % 1023); + code_phase -= cp_int - (cp_int % code_to_chip_num(code)); return code_phase; } @@ -302,6 +305,7 @@ double propagate_code_phase(double code_phase, double carrier_freq, u32 n_sample * \param ref_sample_count NAP sample count at which code_phase was acquired. * \param code_phase Code phase * \param carrier_freq Carrier frequency Doppler (Hz). + * \param chips_to_correlate Chips to correlate. * \param cn0_init Initial C/N0 estimate (dBHz). * \param elevation Elevation (deg). * @@ -309,7 +313,8 @@ double propagate_code_phase(double code_phase, double carrier_freq, u32 n_sample */ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, u32 ref_sample_count, float code_phase, - float carrier_freq, float cn0_init, s8 elevation) + float carrier_freq, u32 chips_to_correlate, + float cn0_init, s8 elevation) { tracker_channel_t *tracker_channel = tracker_channel_get(id); @@ -332,7 +337,8 @@ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, tracker_channel->elevation = elevation; common_data_init(&tracker_channel->common_data, ref_sample_count, - carrier_freq, cn0_init); + carrier_freq, cn0_init, sid.code); + internal_data_init(&tracker_channel->internal_data, sid); interface_function(tracker_channel, tracker_interface->init); @@ -345,7 +351,7 @@ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, tracker_channel_unlock(tracker_channel); nap_track_init(tracker_channel->info.nap_channel, sid, ref_sample_count, - carrier_freq, code_phase); + carrier_freq, code_phase, chips_to_correlate); return true; } @@ -993,9 +999,11 @@ static void event(tracker_channel_t *tracker_channel, event_t event) * \param sample_count Sample count. * \param carrier_freq Carrier frequency. * \param cn0 C/N0 estimate. + * \param code Code identifier. */ static void common_data_init(tracker_common_data_t *common_data, - u32 sample_count, float carrier_freq, float cn0) + u32 sample_count, float carrier_freq, + float cn0, code_t code) { /* Initialize all fields to 0 */ memset(common_data, 0, sizeof(tracker_common_data_t)); @@ -1003,8 +1011,8 @@ static void common_data_init(tracker_common_data_t *common_data, common_data->TOW_ms = TOW_INVALID; /* Calculate code phase rate with carrier aiding. */ - common_data->code_phase_rate = (1 + carrier_freq/GPS_L1_HZ) * - GPS_CA_CHIPPING_RATE; + common_data->code_phase_rate = (1 + carrier_freq / code_to_carr_freq(code)) * + GPS_CA_CHIPPING_RATE; common_data->carrier_freq = carrier_freq; common_data->sample_count = sample_count; diff --git a/src/track.h b/src/track.h index d8afe933..962d057e 100644 --- a/src/track.h +++ b/src/track.h @@ -23,6 +23,7 @@ #include #include "board/nap/track_channel.h" +#include "track_api.h" /** \addtogroup tracking * \{ */ @@ -37,7 +38,8 @@ void track_setup(void); void tracking_send_state(void); -double propagate_code_phase(double code_phase, double carrier_freq, u32 n_samples); +double propagate_code_phase(double code_phase, double carrier_freq, + u32 n_samples, code_t code); /* Update interface */ void tracking_channels_update(u32 channels_mask); @@ -48,7 +50,8 @@ void tracking_channels_missed_update_error(u32 channels_mask); bool tracker_channel_available(tracker_channel_id_t id, gnss_signal_t sid); bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, u32 ref_sample_count, float code_phase, - float carrier_freq, float cn0_init, s8 elevation); + float carrier_freq, u32 chips_to_correlate, + float cn0_init, s8 elevation); bool tracker_channel_disable(tracker_channel_id_t id); /* Tracking parameters interface. diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index b58c0ca7..89ff5f1d 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -11,7 +11,10 @@ */ #include "track_gps_l1ca.h" +#include "track_gps_l2cm.h" /* for L1C/A to L2 CM tracking handover */ #include "track_api.h" +#include "track.h" +#include "decode.h" #include #include @@ -50,6 +53,9 @@ #define CN0_EST_LPF_CUTOFF 5 +/* Convert milliseconds to L1C/A chips */ +#define L1CA_TRACK_MS_TO_CHIPS(ms) ((ms) * GPS_L1CA_CHIPS_NUM) + static struct loop_params { float code_bw, code_zeta, code_k, carr_to_code; float carr_bw, carr_zeta, carr_k, carr_fll_aid_gain; @@ -224,7 +230,8 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, if (!data->short_cycle) { tracker_retune(channel_info->context, common_data->carrier_freq, - common_data->code_phase_rate, 0); + common_data->code_phase_rate, + L1CA_TRACK_MS_TO_CHIPS(1)); return; } } @@ -337,11 +344,19 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, /* Indicate that a mode change has occurred. */ common_data->mode_change_count = common_data->update_count; + + do_l1ca_to_l2cm_handover(common_data->sample_count, + channel_info->sid.sat, + channel_info->nap_channel, + common_data->code_phase_early); } + u32 chips_to_correlate = (1 == data->int_ms) ? + L1CA_TRACK_MS_TO_CHIPS(1) : + L1CA_TRACK_MS_TO_CHIPS(data->int_ms - 1); + tracker_retune(channel_info->context, common_data->carrier_freq, - common_data->code_phase_rate, - data->int_ms == 1 ? 0 : data->int_ms - 2); + common_data->code_phase_rate, chips_to_correlate); } /** Parse a string describing the tracking loop filter parameters into diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index eb3ea059..295e1747 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -19,7 +19,10 @@ #include "track.h" +#define FEATURE_L2C /* skip weak attributes for L2C API implementation */ #include "track_gps_l2cm.h" +#undef FEATURE_L2C + #include "track_api.h" #include "decode.h" #include "manage.h" @@ -36,10 +39,177 @@ #include "settings.h" #include "signal.h" +#define NUM_GPS_L2CM_TRACKERS 32 + +/** L2C coherent integration time [ms] */ +#define L2C_COHERENT_INTEGRATION_TIME_MS 20 + +/* Alias detection interval [ms] */ +#define L2C_ALIAS_DETECT_INTERVAL_MS 500 + /* Number of chips to integrate over in the short cycle interval [chips] * The value must be within [0..(2 * GPS_L2CM_CHIPS_NUM)]. + * 2 * GPS_L2CM_CHIPS_NUM equals to 20ms */ -#define L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS 1023 +#define L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS 300 + +/* Number of chips to integrate over in the short cycle interval [ms] */ +#define L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS \ + (1e3 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS / GPS_CA_CHIPPING_RATE) + +/* Number of chips to integrate over in the long cycle interval [chips] */ +#define L2CM_TRACK_LONG_CYCLE_INTERVAL_CHIPS \ + (2 * GPS_L2CM_CHIPS_NUM - L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS) + +/* Number of chips to integrate over in the long cycle interval [ms] */ +#define L2CM_TRACK_LONG_CYCLE_INTERVAL_MS \ + (L2C_COHERENT_INTEGRATION_TIME_MS - L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS) + +/* Number of chips to integrate over in the long cycle interval [chips] */ +#define L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_CHIPS \ + (2 * GPS_L2CM_CHIPS_NUM - 2 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS) + +/* Number of chips to integrate over in the startup long cycle interval [ms] */ +#define L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_MS \ + (L2C_COHERENT_INTEGRATION_TIME_MS - 2 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS) + +#define L2CM_TRACK_SETTING_SECTION "l2cm_track" + +/* code: nbw zeta k carr_to_code + carrier: nbw zeta k fll_aid */ +#define LOOP_PARAMS_MED "(20 ms, (1, 0.7, 1, 1200), (13, 0.7, 1, 5))" + +/* k1, k2, lp, lo */ +#define LD_PARAMS "0.0247, 1.5, 50, 240" +#define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" + +#define CN0_EST_LPF_CUTOFF 5 + +static struct loop_params { + float code_bw, code_zeta, code_k, carr_to_code; + float carr_bw, carr_zeta, carr_k, carr_fll_aid_gain; + u8 coherent_ms; +} loop_params_stage; + +static struct lock_detect_params { + float k1, k2; + u16 lp, lo; +} lock_detect_params; + +static float track_cn0_use_thres = 31.0; /* dBHz */ +static float track_cn0_drop_thres = 31.0; /* dBHz */ + +static char loop_params_string[120] = LOOP_PARAMS_MED; +static char lock_detect_params_string[24] = LD_PARAMS; +static bool use_alias_detection = true; + +typedef struct { + aided_tl_state_t tl_state; /**< Tracking loop filter state. */ + corr_t cs[3]; /**< EPL correlation results in correlation period. */ + cn0_est_state_t cn0_est; /**< C/N0 Estimator. */ + u8 int_ms; /**< Integration length. */ + bool short_cycle; /**< Set to true when a short 1ms integration is requested. */ + u8 startup; /**< An indicator of start-up phase. */ + u8 stage; /**< 0 = First-stage. 1 ms integration. + 1 = Second-stage. After nav bit sync, + retune loop filters and typically (but + not necessarily) use longer integration. */ + alias_detect_t alias_detect; /**< Alias lock detector. */ + lock_detect_t lock_detect; /**< Phase-lock detector state. */ + u16 sat; /**< Satellite ID */ +} gps_l2cm_tracker_data_t; + +static tracker_t gps_l2cm_trackers[NUM_GPS_L2CM_TRACKERS]; +static gps_l2cm_tracker_data_t gps_l2cm_tracker_data[NUM_GPS_L2CM_TRACKERS]; + +static void tracker_gps_l2cm_init(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data); +static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data); +static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data); + +static bool parse_loop_params(struct setting *s, const char *val); +static bool parse_lock_detect_params(struct setting *s, const char *val); + +static const tracker_interface_t tracker_interface_gps_l2cm = { + .code = CODE_GPS_L2CM, + .init = tracker_gps_l2cm_init, + .disable = tracker_gps_l2cm_disable, + .update = tracker_gps_l2cm_update, + .trackers = gps_l2cm_trackers, + .num_trackers = NUM_GPS_L2CM_TRACKERS +}; + +static tracker_interface_list_element_t + tracker_interface_list_element_gps_l2cm = { + .interface = &tracker_interface_gps_l2cm, + .next = 0 + }; + +/** Register L2 CM tracker into the the tracker interface & settings + * framework. + */ +void track_gps_l2cm_register(void) +{ + SETTING_NOTIFY(L2CM_TRACK_SETTING_SECTION, "loop_params", + loop_params_string, + TYPE_STRING, parse_loop_params); + + SETTING_NOTIFY(L2CM_TRACK_SETTING_SECTION, "lock_detect_params", + lock_detect_params_string, + TYPE_STRING, parse_lock_detect_params); + + SETTING(L2CM_TRACK_SETTING_SECTION, "cn0_use", + track_cn0_use_thres, TYPE_FLOAT); + + SETTING(L2CM_TRACK_SETTING_SECTION, "cn0_drop", + track_cn0_drop_thres, TYPE_FLOAT); + + SETTING(L2CM_TRACK_SETTING_SECTION, "alias_detect", + use_alias_detection, TYPE_BOOL); + + for (u32 i = 0; i < NUM_GPS_L2CM_TRACKERS; i++) { + gps_l2cm_trackers[i].active = false; + gps_l2cm_trackers[i].data = &gps_l2cm_tracker_data[i]; + } + + tracker_interface_register(&tracker_interface_list_element_gps_l2cm); +} + +/** Check if a satellite is already in track + * + * \param sat Satellite ID + * \retval false Satellite is not in track + * \retval true Satellite is in track + */ +static bool is_in_track(u16 sat) +{ + bool ret = false; + gps_l2cm_tracker_data_t* ptr; + u32 i; + + for (i = 0; + i < sizeof(gps_l2cm_trackers) / sizeof(gps_l2cm_trackers[0]); + i++) { + + if (!gps_l2cm_trackers[i].active) { + continue; + } + ptr = (gps_l2cm_tracker_data_t*)gps_l2cm_trackers[i].data; + if (NULL == ptr) { + continue; + } + if (ptr->sat == sat) { + ret = true; + break; + } + } + return ret; +} /** Do L1C/A to L2 CM handover. * @@ -55,6 +225,10 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, u8 nap_channel, float code_phase) { + if (is_in_track(sat)) { + return; /* L2C signal from the SV is already in track */ + } + /* First, get L2C capability for the SV from NDB */ u32 l2c_cpbl; // TODO: uncomment this as soon as NDB gets available @@ -131,3 +305,334 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, log_error_sid(sid, "Failed to start L2C tracking"); } } + +static void tracker_gps_l2cm_init(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data) +{ + (void)channel_info; + gps_l2cm_tracker_data_t *data = tracker_data; + + memset(data, 0, sizeof(gps_l2cm_tracker_data_t)); + tracker_ambiguity_unknown(channel_info->context); + + const struct loop_params *l = &loop_params_stage; + + assert(20 == l->coherent_ms); + data->int_ms = l->coherent_ms; + + aided_tl_init(&(data->tl_state), 1e3 / data->int_ms, + common_data->code_phase_rate - GPS_CA_CHIPPING_RATE, + l->code_bw, l->code_zeta, l->code_k, + l->carr_to_code, + common_data->carrier_freq, + l->carr_bw, l->carr_zeta, l->carr_k, + l->carr_fll_aid_gain); + + data->short_cycle = true; + data->startup = 2; + + /* Initialise C/N0 estimator */ + cn0_est_init(&data->cn0_est, 1e3 / data->int_ms, common_data->cn0, + CN0_EST_LPF_CUTOFF, 1e3 / data->int_ms); + + /* Initialize lock detector */ + lock_detect_init(&data->lock_detect, + lock_detect_params.k1, lock_detect_params.k2, + lock_detect_params.lp, lock_detect_params.lo); + + /* TODO: Reconfigure alias detection between stages */ + u8 alias_detect_ms = l->coherent_ms; + alias_detect_init(&data->alias_detect, + L2C_ALIAS_DETECT_INTERVAL_MS / alias_detect_ms, + (alias_detect_ms - 1) * 1e-3); + + /* Let's remember SV ID to avoid double tracking of L2C from it. */ + data->sat = channel_info->sid.sat; +} + +static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data) +{ + (void)channel_info; + (void)common_data; + (void)tracker_data; +} + +/** Handle tracker start-up stage. + * The total length of coherent integrations is always 20 ms. + * At start-up, the tracker always uses the following integration scheme: + * 'short + short + start-up long'. + * Then it continues with 'short + regular long' cycles. + * The reason behind it is that when tracker gets started in NAP, we always + * use the short integration cycle first, which is + * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS chips long. When the first NAP + * interrupt arrives, the L2C tracker gets the first chance to change the + * integration length. It sets it to be + * L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_CHIPS chips. However, NAP continues + * doing integration and still uses the previous integration length, which is + * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS chips long. At the next interrupt + * FW sets the integration length to be L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS. + * + * \param channel_info INfo associated with a tracker channel + * \param common_data Common tracking channel data + * \param data L2C specific tracking channel data + */ +static void handle_tracker_startup(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + gps_l2cm_tracker_data_t *data) +{ + corr_t cs[3]; + + switch (data->startup) { + case 2: + tracker_correlations_read(channel_info->context, data->cs, + &common_data->sample_count, + &common_data->code_phase_early, + &common_data->carrier_phase); + alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); + + tracker_retune(channel_info->context, common_data->carrier_freq, + common_data->code_phase_rate, + L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_CHIPS); + break; + + case 1: + tracker_correlations_read(channel_info->context, cs, + &common_data->sample_count, + &common_data->code_phase_early, + &common_data->carrier_phase); + /* Accumulate two short cycle correlations */ + for(int i = 0; i < 3; i++) { + data->cs[i].I += cs[i].I; + data->cs[i].Q += cs[i].Q; + } + tracker_retune(channel_info->context, common_data->carrier_freq, + common_data->code_phase_rate, + L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS); + break; + + default: + assert(0); + break; + } +} + +static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, + tracker_common_data_t *common_data, + tracker_data_t *tracker_data) +{ + gps_l2cm_tracker_data_t *data = tracker_data; + + if (data->startup) { + /* Handle start-up case, when we have 1+1+18 integration cycles */ + handle_tracker_startup(channel_info, common_data, data); + if (1 == data->startup) { + // The start-up phase is over. Continue with the normal + // sequence of short & long cycles. The next cycle + // is going to be long. + data->short_cycle = false; // start-up phase is over + } + if (data->startup) { + data->startup--; + } + return; + } + + /* Read early ([0]), prompt ([1]) and late ([2]) correlations. */ + if (data->short_cycle) { + tracker_correlations_read(channel_info->context, data->cs, + &common_data->sample_count, + &common_data->code_phase_early, + &common_data->carrier_phase); + alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); + } else { + /* This is the end of the long cycle's correlations. */ + corr_t cs[3]; + tracker_correlations_read(channel_info->context, cs, + &common_data->sample_count, + &common_data->code_phase_early, + &common_data->carrier_phase); + /* Accumulate short cycle correlations with long ones. */ + for(int i = 0; i < 3; i++) { + data->cs[i].I += cs[i].I; + data->cs[i].Q += cs[i].Q; + } + } + + u8 int_ms = data->short_cycle ? + L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS : + L2CM_TRACK_LONG_CYCLE_INTERVAL_MS; + common_data->TOW_ms = tracker_tow_update(channel_info->context, + common_data->TOW_ms, + int_ms); + + /* We're doing long integrations, alternate between short and long + * cycles. This is because of FPGA pipelining and latency. + * The loop parameters can only be updated at the end of the second + * integration interval. + */ + bool short_cycle = data->short_cycle; + + data->short_cycle = !data->short_cycle; + + if (short_cycle) { + tracker_retune(channel_info->context, common_data->carrier_freq, + common_data->code_phase_rate, + L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS); + return; + } + + common_data->update_count += data->int_ms; + + tracker_bit_sync_update(channel_info->context, data->int_ms, data->cs[1].I); + + corr_t* cs = data->cs; + + /* Update C/N0 estimate */ + common_data->cn0 = cn0_est(&data->cn0_est, + cs[1].I / data->int_ms, + cs[1].Q / data->int_ms); + if (common_data->cn0 > track_cn0_drop_thres) { + common_data->cn0_above_drop_thres_count = common_data->update_count; + } + + if (common_data->cn0 < track_cn0_use_thres) { + /* SNR has dropped below threshold, indicate that the carrier phase + * ambiguity is now unknown as cycle slips are likely. */ + tracker_ambiguity_unknown(channel_info->context); + /* Update the latest time we were below the threshold. */ + common_data->cn0_below_use_thres_count = common_data->update_count; + } + + /* Update PLL lock detector */ + bool last_outp = data->lock_detect.outp; + lock_detect_update(&data->lock_detect, cs[1].I, cs[1].Q, data->int_ms); + if (data->lock_detect.outo) { + common_data->ld_opti_locked_count = common_data->update_count; + } + if (!data->lock_detect.outp) { + common_data->ld_pess_unlocked_count = common_data->update_count; + } + + /* Reset carrier phase ambiguity if there's doubt as to our phase lock */ + if (last_outp && !data->lock_detect.outp) { + log_info_sid(channel_info->sid, "PLL stress"); + tracker_ambiguity_unknown(channel_info->context); + } + + /* Run the loop filters. */ + + /* Output I/Q correlations using SBP if enabled for this channel */ + tracker_correlations_send(channel_info->context, cs); + + correlation_t cs2[3]; + for (u32 i = 0; i < 3; i++) { + cs2[i].I = cs[2-i].I; + cs2[i].Q = cs[2-i].Q; + } + + aided_tl_update(&data->tl_state, cs2); + common_data->carrier_freq = data->tl_state.carr_freq; + common_data->code_phase_rate = data->tl_state.code_freq + + GPS_CA_CHIPPING_RATE; + + /* Attempt alias detection if we have pessimistic phase lock detect OR + optimistic phase lock detect */ + if (use_alias_detection && + (data->lock_detect.outp || data->lock_detect.outo)) { + s32 I = (cs[1].I - data->alias_detect.first_I) / (data->int_ms - 1); + s32 Q = (cs[1].Q - data->alias_detect.first_Q) / (data->int_ms - 1); + float err = alias_detect_second(&data->alias_detect, I, Q); + if (fabs(err) > (250 / data->int_ms)) { + if (data->lock_detect.outp) { + log_warn_sid(channel_info->sid, "False phase lock detected"); + } + + tracker_ambiguity_unknown(channel_info->context); + /* Indicate that a mode change has occurred. */ + common_data->mode_change_count = common_data->update_count; + + data->tl_state.carr_freq += err; + data->tl_state.carr_filt.y = data->tl_state.carr_freq; + } + } + + tracker_retune(channel_info->context, common_data->carrier_freq, + common_data->code_phase_rate, + L2CM_TRACK_LONG_CYCLE_INTERVAL_CHIPS); +} + +/** Parse a string describing the tracking loop filter parameters into + * the loop_params_stage struct. + * + * \param s Settings structure provided to store the input string. + * \param val The input string to parse. + * \retval true Success + * \retval false Failure + */ +static bool parse_loop_params(struct setting *s, const char *val) +{ + /** The string contains loop parameters for one stage */ + + struct loop_params loop_params_parse; + + const char *str = val; + struct loop_params *l = &loop_params_parse; + + unsigned int tmp; /* newlib's sscanf doesn't support hh size modifier */ + + if (sscanf(str, "( %u ms , ( %f , %f , %f , %f ) , ( %f , %f , %f , %f ) ) ", + &tmp, + &l->code_bw, &l->code_zeta, &l->code_k, &l->carr_to_code, + &l->carr_bw, &l->carr_zeta, &l->carr_k, &l->carr_fll_aid_gain + ) < 9) { + log_error("Ill-formatted tracking loop param string: %20s", str); + return false; + } + l->coherent_ms = tmp; + + if (l->coherent_ms != L2C_COHERENT_INTEGRATION_TIME_MS) { + log_error("Invalid coherent integration length for L2CM: %" PRIu8, + l->coherent_ms); + return false; + } + /* Successfully parsed the input. Save to memory. */ + strncpy(s->addr, val, s->len); + if (s->len > 0) { + char *ptr = (char*) s->addr; + ptr[s->len - 1] = '\0'; + } + memcpy(&loop_params_stage, &loop_params_parse, sizeof(loop_params_stage)); + + return true; +} + +/** Parse a string describing the tracking loop phase lock detector + * parameters into the lock_detect_params structs. + * + * \param s Settings structure provided to store the input string. + * \param val The input string to parse. + * \retval true Success + * \retval false Failure + */ +static bool parse_lock_detect_params(struct setting *s, const char *val) +{ + struct lock_detect_params p; + + if (sscanf(val, "%f , %f , %" SCNu16 " , %" SCNu16, + &p.k1, &p.k2, &p.lp, &p.lo) < 4) { + log_error("Ill-formatted lock detect param string: %20s", val); + return false; + } + /* Successfully parsed the input. Save to memory. */ + strncpy(s->addr, val, s->len); + if (s->len > 0) { + char *ptr = (char*) s->addr; + ptr[s->len - 1] = '\0'; + } + memcpy(&lock_detect_params, &p, sizeof(lock_detect_params)); + + return true; +} diff --git a/src/track/track_gps_l2cm.h b/src/track/track_gps_l2cm.h index 431784b2..57ea2d61 100644 --- a/src/track/track_gps_l2cm.h +++ b/src/track/track_gps_l2cm.h @@ -15,9 +15,19 @@ #include +#ifdef FEATURE_L2C +# define L2C_WEAK +#else +# define L2C_WEAK __attribute__ ((weak, alias ("l2c_not_implemented"))) +#endif /* FEATURE_L2C */ + +int l2c_not_implemented() __attribute__ ((weak)); +inline int l2c_not_implemented() { return -1; } + +void track_gps_l2cm_register(void) L2C_WEAK; void do_l1ca_to_l2cm_handover(u32 sample_count, u16 sat, u8 nap_channel, - float code_phase); + float code_phase) L2C_WEAK; #endif /* SWIFTNAV_TRACK_GPS_L2CM_H */ diff --git a/src/track_api.c b/src/track_api.c index cd3a010f..85c55eca 100644 --- a/src/track_api.c +++ b/src/track_api.c @@ -74,18 +74,18 @@ void tracker_correlations_read(tracker_context_t *context, corr_t *cs, * \param context Tracker context. * \param carrier_freq Carrier frequency (Hz). * \param code_phase_rate Code phase rate (chips/s). - * \param rollover_count Number of code cycles to integrate over. + * \param chips_to_correlate Number of code chips to integrate over. */ void tracker_retune(tracker_context_t *context, double carrier_freq, - double code_phase_rate, u8 rollover_count) + double code_phase_rate, u32 chips_to_correlate) { const tracker_channel_info_t *channel_info; tracker_internal_data_t *internal_data; tracker_internal_context_resolve(context, &channel_info, &internal_data); /* Write NAP UPDATE register. */ - nap_track_update(channel_info->nap_channel, carrier_freq, - code_phase_rate, rollover_count, 0); + nap_track_update(channel_info->nap_channel, + carrier_freq, code_phase_rate, chips_to_correlate, 0); } /** Update the TOW for a tracker channel. @@ -158,6 +158,13 @@ void tracker_bit_sync_update(tracker_context_t *context, u32 int_ms, tracker_internal_data_t *internal_data; tracker_internal_context_resolve(context, &channel_info, &internal_data); + /* L2C bit sync is known once we start tracking it since + ranging code length matches bit length (20ms) */ + if (CODE_GPS_L2CM == channel_info->sid.code) { + internal_data->bit_sync.bit_phase_ref = ~(BITSYNC_UNSYNCED); + return; + } + /* Update bit sync */ s32 bit_integrate; if (bit_sync_update(&internal_data->bit_sync, corr_prompt_real, int_ms, diff --git a/src/track_api.h b/src/track_api.h index 5b41ad48..b1d7b439 100644 --- a/src/track_api.h +++ b/src/track_api.h @@ -104,7 +104,7 @@ void tracker_correlations_read(tracker_context_t *context, corr_t *cs, u32 *sample_count, double *code_phase, double *carrier_phase); void tracker_retune(tracker_context_t *context, double carrier_freq, - double code_phase_rate, u8 rollover_count); + double code_phase_rate, u32 chips_to_correlate); s32 tracker_tow_update(tracker_context_t *context, s32 current_TOW_ms, u32 int_ms); void tracker_bit_sync_update(tracker_context_t *context, u32 int_ms, From db7d81f58559011e82490d135d57e05c41d7edf8 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Mon, 16 May 2016 17:07:32 +0300 Subject: [PATCH 07/21] Address code review comments --- src/manage.c | 31 +++++++++++++++----- src/manage.h | 1 + src/sbp_utils.c | 2 +- src/track/track_gps_l2cm.c | 58 +++++++++++--------------------------- src/track_api.c | 21 +++++++++----- src/track_api.h | 1 + 6 files changed, 58 insertions(+), 56 deletions(-) diff --git a/src/manage.c b/src/manage.c index 1f571cc4..107f3561 100644 --- a/src/manage.c +++ b/src/manage.c @@ -193,13 +193,6 @@ void manage_acq_setup() track_mask[i] = false; almanac[i].valid = 0; - if (CODE_GPS_L2CM == acq_status[i].sid.code) { - /* Do not acquire GPS L2C. - * Do GPS L1 C/A to L2C handover at the tracking stage instead. */ - acq_status[i].state = ACQ_PRN_SKIP; - acq_status[i].masked = true; - } - if (!sbas_enabled && (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) { acq_status[i].masked = true; @@ -314,6 +307,12 @@ static acq_status_t * choose_acq_sat(void) gps_time_t t = get_current_time(); for (u32 i=0; istate == ACQ_PRN_ACQUIRING) && (!acq->masked); +} + /** Queue a request to start up tracking and decoding for the specified sid. * * \note This function is thread-safe and non-blocking. @@ -686,6 +698,11 @@ static void manage_tracking_startup(void) acq_status_t *acq = &acq_status[sid_to_global_index(startup_params.sid)]; + /* Make sure the SID is not already tracked. */ + if (acq->state == ACQ_PRN_TRACKING) { + continue; + } + /* Make sure a tracking channel and a decoder channel are available */ u8 chan = manage_track_new_acq(startup_params.sid); if (chan == MANAGE_NO_CHANNELS_FREE) { diff --git a/src/manage.h b/src/manage.h index fd8ca59b..225351b7 100644 --- a/src/manage.h +++ b/src/manage.h @@ -82,6 +82,7 @@ void manage_track_setup(void); s8 use_tracking_channel(u8 i); u8 tracking_channels_ready(void); +bool tracking_startup_ready(gnss_signal_t sid); bool tracking_startup_request(const tracking_startup_params_t *startup_params); #endif diff --git a/src/sbp_utils.c b/src/sbp_utils.c index 305cc7f4..9a0be85b 100644 --- a/src/sbp_utils.c +++ b/src/sbp_utils.c @@ -294,7 +294,7 @@ void unpack_ephemeris(const msg_ephemeris_t *msg, ephemeris_t *e) e->sid = sid_from_sbp(msg->sid); e->kepler.iode = msg->iode; e->kepler.iodc = msg->iodc; - e->fit_interval = (u8)(4 * 60 * 60); /* TODO: this is a work around until SBP updated */ + e->fit_interval = 4 * 60 * 60; /* TODO: this is a work around until SBP updated */ e->ura = 2.0f; /* TODO: this is a work around until SBP updated*/ } diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index 295e1747..d3d14656 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -180,37 +180,6 @@ void track_gps_l2cm_register(void) tracker_interface_register(&tracker_interface_list_element_gps_l2cm); } -/** Check if a satellite is already in track - * - * \param sat Satellite ID - * \retval false Satellite is not in track - * \retval true Satellite is in track - */ -static bool is_in_track(u16 sat) -{ - bool ret = false; - gps_l2cm_tracker_data_t* ptr; - u32 i; - - for (i = 0; - i < sizeof(gps_l2cm_trackers) / sizeof(gps_l2cm_trackers[0]); - i++) { - - if (!gps_l2cm_trackers[i].active) { - continue; - } - ptr = (gps_l2cm_tracker_data_t*)gps_l2cm_trackers[i].data; - if (NULL == ptr) { - continue; - } - if (ptr->sat == sat) { - ret = true; - break; - } - } - return ret; -} - /** Do L1C/A to L2 CM handover. * * The condition for the handover is the availability of bitsync on L1 C/A @@ -225,10 +194,6 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, u8 nap_channel, float code_phase) { - if (is_in_track(sat)) { - return; /* L2C signal from the SV is already in track */ - } - /* First, get L2C capability for the SV from NDB */ u32 l2c_cpbl; // TODO: uncomment this as soon as NDB gets available @@ -259,10 +224,11 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, l2c_cpbl |= (u32)1 << (32 - 1); /* compose SID: same SV, but code is L2 CM */ - gnss_signal_t sid = { - .sat = sat, - .code = CODE_GPS_L2CM - }; + gnss_signal_t sid = construct_sid(CODE_GPS_L2CM, sat); + + if (!tracking_startup_ready(sid)) { + return; /* L2C signal from the SV is already in track */ + } if (0 == (l2c_cpbl & ((u32)1 << (sat - 1)))) { log_info_sid(sid, "SV does not support L2C signal"); @@ -287,7 +253,10 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, /* get initial cn0 from parent L1 channel */ float cn0_init = tracking_channel_cn0_get(nap_channel); - s8 elevation = tracking_channel_evelation_degrees_get(nap_channel); + /* The best elevation estimation could be retrieved by calling + tracking_channel_evelation_degrees_get(nap_channel) here. + However, we assume it is done where tracker_channel_init() + is called. */ tracking_startup_params_t startup_params = { .sid = sid, @@ -296,7 +265,7 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, .code_phase = code_phase, .chips_to_correlate = L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS, .cn0_init = cn0_init, - .elevation = elevation + .elevation = TRACKING_ELEVATION_UNKNOWN }; if (tracking_startup_request(&startup_params)) { @@ -486,6 +455,13 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, common_data->update_count += data->int_ms; + /* L2C bit sync is known once we start tracking it since + the L2C ranging code length matches the bit length (20ms). + This is the end of 20ms integration period and the edge + of a data bit. */ + tracker_bit_sync_set(channel_info->context, 0); + + /* Call the bit sync update API to do data decoding */ tracker_bit_sync_update(channel_info->context, data->int_ms, data->cs[1].I); corr_t* cs = data->cs; diff --git a/src/track_api.c b/src/track_api.c index 85c55eca..e43714d6 100644 --- a/src/track_api.c +++ b/src/track_api.c @@ -145,6 +145,20 @@ s32 tracker_tow_update(tracker_context_t *context, s32 current_TOW_ms, return current_TOW_ms; } +/** Set bit sync phase reference + * + * \param context Tracker context. + * \param bit_phase_ref Bit phase reference. + */ +void tracker_bit_sync_set(tracker_context_t *context, s8 bit_phase_ref) +{ + const tracker_channel_info_t *channel_info; + tracker_internal_data_t *internal_data; + tracker_internal_context_resolve(context, &channel_info, &internal_data); + + bit_sync_set(&internal_data->bit_sync, bit_phase_ref); +} + /** Update bit sync and output navigation message bits for a tracker channel. * * \param context Tracker context. @@ -158,13 +172,6 @@ void tracker_bit_sync_update(tracker_context_t *context, u32 int_ms, tracker_internal_data_t *internal_data; tracker_internal_context_resolve(context, &channel_info, &internal_data); - /* L2C bit sync is known once we start tracking it since - ranging code length matches bit length (20ms) */ - if (CODE_GPS_L2CM == channel_info->sid.code) { - internal_data->bit_sync.bit_phase_ref = ~(BITSYNC_UNSYNCED); - return; - } - /* Update bit sync */ s32 bit_integrate; if (bit_sync_update(&internal_data->bit_sync, corr_prompt_real, int_ms, diff --git a/src/track_api.h b/src/track_api.h index b1d7b439..39b5439b 100644 --- a/src/track_api.h +++ b/src/track_api.h @@ -107,6 +107,7 @@ void tracker_retune(tracker_context_t *context, double carrier_freq, double code_phase_rate, u32 chips_to_correlate); s32 tracker_tow_update(tracker_context_t *context, s32 current_TOW_ms, u32 int_ms); +void tracker_bit_sync_set(tracker_context_t *context, s8 bit_phase_ref); void tracker_bit_sync_update(tracker_context_t *context, u32 int_ms, s32 corr_prompt_real); u8 tracker_bit_length_get(tracker_context_t *context); From 4b8355360a54b634ce51178fdf009765254c1897 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Wed, 18 May 2016 11:23:04 +0300 Subject: [PATCH 08/21] Address review comments (round 2) --- src/board/v3/nap/track_channel.c | 70 ++------------------------------ src/board/v3/platform_signal.h | 1 + src/track.c | 2 +- src/track/track_gps_l1ca.c | 5 ++- src/track/track_gps_l2cm.c | 49 +++++++++------------- src/track/track_gps_l2cm.h | 9 ++-- 6 files changed, 33 insertions(+), 103 deletions(-) diff --git a/src/board/v3/nap/track_channel.c b/src/board/v3/nap/track_channel.c index 6c309b7c..b766799f 100644 --- a/src/board/v3/nap/track_channel.c +++ b/src/board/v3/nap/track_channel.c @@ -23,6 +23,7 @@ #include #include #include +#include /* to expose sid_to_init_g1() declaration */ #include #include @@ -72,72 +73,6 @@ static u32 calc_length_samples(u32 chips_to_correlate, u32 cp_start_frac_units, return samples; } -/** Return the init value of NAP G1 pseudorandom generator. - * \param sid Signal ID - * \return NAP G1 initial value - */ -u32 sid_to_init_g1(gnss_signal_t sid) -{ - u32 ret = ~0; - /* The L2C G1 init values are taken from IS-GPS-200H Table 3-IIa - "Code phase assignments (IIR-M, IIF, and subsequent blocks only)" - For example, PRN 1 has G1 init value 0742417664. - */ - u32 gps_l2cm_prns_init_values[] = { - /* 0 */ 0742417664, - /* 1 */ 0756014035, - /* 2 */ 0002747144, - /* 3 */ 0066265724, - /* 4 */ 0601403471, - /* 5 */ 0703232733, - /* 6 */ 0124510070, - /* 7 */ 0617316361, - /* 8 */ 0047541621, - /* 9 */ 0733031046, - /* 10 */ 0713512145, - /* 11 */ 0024437606, - /* 12 */ 0021264003, - /* 13 */ 0230655351, - /* 14 */ 0001314400, - /* 15 */ 0222021506, - /* 16 */ 0540264026, - /* 17 */ 0205521705, - /* 18 */ 0064022144, - /* 19 */ 0120161274, - /* 20 */ 0044023533, - /* 21 */ 0724744327, - /* 22 */ 0045743577, - /* 23 */ 0741201660, - /* 24 */ 0700274134, - /* 25 */ 0010247261, - /* 26 */ 0713433445, - /* 27 */ 0737324162, - /* 28 */ 0311627434, - /* 29 */ 0710452007, - /* 30 */ 0722462133, - /* 31 */ 0050172213, - /* 32 */ 0500653703, - /* 33 */ 0755077436, - /* 34 */ 0136717361, - /* 35 */ 0756675453, - /* 36 */ 0435506112 - }; - - switch (sid.code) { - case CODE_GPS_L2CM: - ret = gps_l2cm_prns_init_values[sid.sat - 1] & 0x7FFFFFF; - break; - case CODE_GPS_L1CA: - ret = 0x3ff; - break; - default: - assert(0); - break; - } - - return ret; -} - /** Looks-up RF frontend channel for the given signal ID. * \param sid Signal ID. * \return RF front-end channel number. @@ -193,6 +128,7 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, u16 control; u8 prn = sid.sat - GPS_FIRST_PRN; + /* PRN code */ control = (prn << NAP_TRK_CONTROL_SAT_Pos) & NAP_TRK_CONTROL_SAT_Msk; /* RF frontend channel */ @@ -239,7 +175,7 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ tc_req += (NAP_FRONTEND_SAMPLE_RATE_Hz / code_to_chip_rate(sid.code)) * - (code_to_chip_num(sid.code) - cp) * + (code_to_chip_count(sid.code) - cp) * (1.0 + carrier_freq / code_to_carr_freq(sid.code)); NAP->TRK_TIMING_COMPARE = tc_req; diff --git a/src/board/v3/platform_signal.h b/src/board/v3/platform_signal.h index 8da7a6fa..c56af53a 100644 --- a/src/board/v3/platform_signal.h +++ b/src/board/v3/platform_signal.h @@ -23,6 +23,7 @@ /* Tracker configuration */ #define NUM_TRACKER_CHANNELS NAP_MAX_N_TRACK_CHANNELS #define NUM_GPS_L1CA_TRACKERS NAP_MAX_N_TRACK_CHANNELS +#define NUM_GPS_L2CM_TRACKERS NAP_MAX_N_TRACK_CHANNELS /* Decoder configuration */ #define NUM_DECODER_CHANNELS NAP_MAX_N_TRACK_CHANNELS diff --git a/src/track.c b/src/track.c index a94175bd..cb4c6888 100644 --- a/src/track.c +++ b/src/track.c @@ -294,7 +294,7 @@ double propagate_code_phase(double code_phase, double carrier_freq, code_to_chip_rate(code); code_phase += n_samples * code_phase_rate / NAP_FRONTEND_SAMPLE_RATE_Hz; u32 cp_int = floor(code_phase); - code_phase -= cp_int - (cp_int % code_to_chip_num(code)); + code_phase -= cp_int - (cp_int % code_to_chip_count(code)); return code_phase; } diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index 89ff5f1d..af87aa2d 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -347,8 +347,9 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, do_l1ca_to_l2cm_handover(common_data->sample_count, channel_info->sid.sat, - channel_info->nap_channel, - common_data->code_phase_early); + common_data->code_phase_early, + common_data->carrier_freq, + common_data->cn0); } u32 chips_to_correlate = (1 == data->int_ms) ? diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index d3d14656..dc071d9f 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -19,9 +19,9 @@ #include "track.h" -#define FEATURE_L2C /* skip weak attributes for L2C API implementation */ +/* skip weak attributes for L2C API implementation */ +#define FEATURE_TRACK_GPS_L2CM #include "track_gps_l2cm.h" -#undef FEATURE_L2C #include "track_api.h" #include "decode.h" @@ -39,8 +39,6 @@ #include "settings.h" #include "signal.h" -#define NUM_GPS_L2CM_TRACKERS 32 - /** L2C coherent integration time [ms] */ #define L2C_COHERENT_INTEGRATION_TIME_MS 20 @@ -116,7 +114,6 @@ typedef struct { not necessarily) use longer integration. */ alias_detect_t alias_detect; /**< Alias lock detector. */ lock_detect_t lock_detect; /**< Phase-lock detector state. */ - u16 sat; /**< Satellite ID */ } gps_l2cm_tracker_data_t; static tracker_t gps_l2cm_trackers[NUM_GPS_L2CM_TRACKERS]; @@ -186,13 +183,15 @@ void track_gps_l2cm_register(void) * * \param sample_count NAP sample count * \param sat L1C/A Satellite ID - * \param nap_channel Associated NAP channel * \param code_phase L1CA code phase [chips] + * \param carrier_freq The current Doppler frequency for the L1 C/A channel + * \param cn0 CN0 estimate for the L1 C/A channel */ void do_l1ca_to_l2cm_handover(u32 sample_count, u16 sat, - u8 nap_channel, - float code_phase) + float code_phase, + double carrier_freq, + float cn0_init) { /* First, get L2C capability for the SV from NDB */ u32 l2c_cpbl; @@ -246,13 +245,6 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, code_phase = 2 * GPS_L2CM_CHIPS_NUM - (GPS_L1CA_CHIPS_NUM - code_phase); } - /* recalculate doppler freq for L2 from L1*/ - double carrier_freq = tracking_channel_carrier_freq_get(nap_channel) * - GPS_L2_HZ / GPS_L1_HZ; - - /* get initial cn0 from parent L1 channel */ - float cn0_init = tracking_channel_cn0_get(nap_channel); - /* The best elevation estimation could be retrieved by calling tracking_channel_evelation_degrees_get(nap_channel) here. However, we assume it is done where tracker_channel_init() @@ -261,9 +253,11 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, tracking_startup_params_t startup_params = { .sid = sid, .sample_count = sample_count, - .carrier_freq = carrier_freq, + /* recalculate doppler freq for L2 from L1*/ + .carrier_freq = carrier_freq * GPS_L2_HZ / GPS_L1_HZ, .code_phase = code_phase, .chips_to_correlate = L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS, + /* get initial cn0 from parent L1 channel */ .cn0_init = cn0_init, .elevation = TRACKING_ELEVATION_UNKNOWN }; @@ -271,7 +265,7 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, if (tracking_startup_request(&startup_params)) { log_info_sid(sid, "L2 CM handover done"); } else { - log_error_sid(sid, "Failed to start L2C tracking"); + log_warn_sid(sid, "Failed to start L2C tracking"); } } @@ -316,8 +310,11 @@ static void tracker_gps_l2cm_init(const tracker_channel_info_t *channel_info, L2C_ALIAS_DETECT_INTERVAL_MS / alias_detect_ms, (alias_detect_ms - 1) * 1e-3); - /* Let's remember SV ID to avoid double tracking of L2C from it. */ - data->sat = channel_info->sid.sat; + /* L2C bit sync is known once we start tracking it since + the L2C ranging code length matches the bit length (20ms). + This is the end of 20ms integration period and the edge + of a data bit. */ + tracker_bit_sync_set(channel_info->context, 0); } static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, @@ -398,10 +395,10 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, /* Handle start-up case, when we have 1+1+18 integration cycles */ handle_tracker_startup(channel_info, common_data, data); if (1 == data->startup) { - // The start-up phase is over. Continue with the normal - // sequence of short & long cycles. The next cycle - // is going to be long. - data->short_cycle = false; // start-up phase is over + /* The start-up phase is over. Continue with the normal + sequence of short & long cycles. The next cycle + is going to be long. */ + data->short_cycle = false; } if (data->startup) { data->startup--; @@ -455,12 +452,6 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, common_data->update_count += data->int_ms; - /* L2C bit sync is known once we start tracking it since - the L2C ranging code length matches the bit length (20ms). - This is the end of 20ms integration period and the edge - of a data bit. */ - tracker_bit_sync_set(channel_info->context, 0); - /* Call the bit sync update API to do data decoding */ tracker_bit_sync_update(channel_info->context, data->int_ms, data->cs[1].I); diff --git a/src/track/track_gps_l2cm.h b/src/track/track_gps_l2cm.h index 57ea2d61..ad8e9c9c 100644 --- a/src/track/track_gps_l2cm.h +++ b/src/track/track_gps_l2cm.h @@ -15,11 +15,11 @@ #include -#ifdef FEATURE_L2C +#ifdef FEATURE_TRACK_GPS_L2CM # define L2C_WEAK #else # define L2C_WEAK __attribute__ ((weak, alias ("l2c_not_implemented"))) -#endif /* FEATURE_L2C */ +#endif /* FEATURE_TRACK_GPS_L2CM */ int l2c_not_implemented() __attribute__ ((weak)); inline int l2c_not_implemented() { return -1; } @@ -27,7 +27,8 @@ inline int l2c_not_implemented() { return -1; } void track_gps_l2cm_register(void) L2C_WEAK; void do_l1ca_to_l2cm_handover(u32 sample_count, u16 sat, - u8 nap_channel, - float code_phase) L2C_WEAK; + float code_phase, + double carrier_freq, + float cn0_init) L2C_WEAK; #endif /* SWIFTNAV_TRACK_GPS_L2CM_H */ From 4c9696a19717cd0efb3d91c6c5d6b3e8111cfbc0 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Wed, 18 May 2016 14:44:23 +0300 Subject: [PATCH 09/21] Do not clobber common_data on short cycle --- src/track/track_gps_l2cm.c | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index dc071d9f..764391a0 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -408,10 +408,22 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, /* Read early ([0]), prompt ([1]) and late ([2]) correlations. */ if (data->short_cycle) { + /* The throw away data. Not needed for the short cycle. + And we also do not want to clobber common_data content by + by writing these data to it as it contains valid data for + the previous full 20 ms cycle. */ + u32 sample_count; /**< Total num samples channel has tracked for. */ + double code_phase_early; /**< Early code phase. */ + double carrier_phase; /**< Carrier phase in NAP register units. */ + tracker_correlations_read(channel_info->context, data->cs, - &common_data->sample_count, - &common_data->code_phase_early, - &common_data->carrier_phase); + &sample_count, + &code_phase_early, + &carrier_phase); + (void) sample_count; + (void) code_phase_early; + (void) carrier_phase; + alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); } else { /* This is the end of the long cycle's correlations. */ From 6af054fe98fd762708f251977926084af2f4cca5 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Thu, 26 May 2016 12:44:38 +0300 Subject: [PATCH 10/21] Address review comments (round 3) --- src/board/v3/nap/nap_hw.h | 4 ++-- src/board/v3/nap/track_channel.c | 8 ++++---- src/manage.c | 8 +++++--- src/track/track_gps_l2cm.c | 11 ++++------- src/track/track_gps_l2cm.h | 8 +++++--- 5 files changed, 20 insertions(+), 19 deletions(-) diff --git a/src/board/v3/nap/nap_hw.h b/src/board/v3/nap/nap_hw.h index d3368d2a..b3f44e99 100644 --- a/src/board/v3/nap/nap_hw.h +++ b/src/board/v3/nap/nap_hw.h @@ -137,8 +137,8 @@ enum { /* The values of NAP_TRKx_CONTROL::CODE field */ enum { - NAP_CONSTELLATION_BAND_1 = 0, /* GPS L1C/A, SBAS L1CA */ - NAP_CONSTELLATION_BAND_2 /* GPS L2CM */ + NAP_CODE_GPS_L1CA_SBAS_L1CA = 0, /* GPS L1C/A, SBAS L1CA */ + NAP_CODE_GPS_L2CM /* GPS L2CM */ }; /* Instances */ diff --git a/src/board/v3/nap/track_channel.c b/src/board/v3/nap/track_channel.c index b766799f..8730a134 100644 --- a/src/board/v3/nap/track_channel.c +++ b/src/board/v3/nap/track_channel.c @@ -105,10 +105,10 @@ u8 sid_to_nap_code(gnss_signal_t sid) switch (sid.code) { case CODE_GPS_L1CA: case CODE_SBAS_L1CA: - ret = NAP_CONSTELLATION_BAND_1; + ret = NAP_CODE_GPS_L1CA_SBAS_L1CA; break; case CODE_GPS_L2CM: - ret = NAP_CONSTELLATION_BAND_2; + ret = NAP_CODE_GPS_L2CM; break; default: assert(0); @@ -132,10 +132,10 @@ void nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, /* PRN code */ control = (prn << NAP_TRK_CONTROL_SAT_Pos) & NAP_TRK_CONTROL_SAT_Msk; /* RF frontend channel */ - control |= (sid_to_rf_frontend_channel(sid) << NAP_TRK_CONTROL_RF_FE_Pos) & \ + control |= (sid_to_rf_frontend_channel(sid) << NAP_TRK_CONTROL_RF_FE_Pos) & NAP_TRK_CONTROL_RF_FE_Msk; /* Constellation and band for tracking */ - control |= (sid_to_nap_code(sid) << NAP_TRK_CONTROL_CODE_Pos) & \ + control |= (sid_to_nap_code(sid) << NAP_TRK_CONTROL_CODE_Pos) & NAP_TRK_CONTROL_CODE_Msk; /* We are not utilizing multiple signals within one RF channel at the moment. Therefore, RF_FE_CH is 0 and below statement in a NOP. */ diff --git a/src/manage.c b/src/manage.c index 107f3561..c80e0165 100644 --- a/src/manage.c +++ b/src/manage.c @@ -307,9 +307,7 @@ static acq_status_t * choose_acq_sat(void) gps_time_t t = get_current_time(); for (u32 i=0; icontext, data->cs, &sample_count, &code_phase_early, &carrier_phase); - (void) sample_count; - (void) code_phase_early; - (void) carrier_phase; alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); } else { diff --git a/src/track/track_gps_l2cm.h b/src/track/track_gps_l2cm.h index ad8e9c9c..40fdba62 100644 --- a/src/track/track_gps_l2cm.h +++ b/src/track/track_gps_l2cm.h @@ -15,16 +15,18 @@ #include -#ifdef FEATURE_TRACK_GPS_L2CM +#ifdef TRACK_GPS_L2CM_INTERNAL # define L2C_WEAK #else # define L2C_WEAK __attribute__ ((weak, alias ("l2c_not_implemented"))) -#endif /* FEATURE_TRACK_GPS_L2CM */ +#endif /* TRACK_GPS_L2CM_INTERNAL */ int l2c_not_implemented() __attribute__ ((weak)); inline int l2c_not_implemented() { return -1; } -void track_gps_l2cm_register(void) L2C_WEAK; +/* not weak as it is used in L2C builds only */ +void track_gps_l2cm_register(void); + void do_l1ca_to_l2cm_handover(u32 sample_count, u16 sat, float code_phase, From 048549ddcdbc7c621dac180c5d5bb4993d436108 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Fri, 27 May 2016 10:35:57 +0300 Subject: [PATCH 11/21] Address review comments (round 4) --- src/track.c | 4 ++-- src/track/track_gps_l2cm.c | 39 ++++++++++++-------------------------- 2 files changed, 14 insertions(+), 29 deletions(-) diff --git a/src/track.c b/src/track.c index cb4c6888..bd202171 100644 --- a/src/track.c +++ b/src/track.c @@ -594,9 +594,9 @@ void tracking_channel_measurement_get(tracker_channel_id_t id, u64 ref_tc, gps_time_t tor = rx2gpstime(ref_tc + meas->rec_time_delta); gps_time_t tot; tot.tow = 1e-3 * meas->time_of_week_ms; - tot.tow += meas->code_phase_chips / GPS_CA_CHIPPING_RATE; + tot.tow += meas->code_phase_chips / code_to_chip_rate(meas->sid.code); gps_time_match_weeks(&tot, &tor); - internal_data->carrier_phase_offset = round(GPS_L1_HZ + internal_data->carrier_phase_offset = round(code_to_carr_freq(meas->sid.code) * gpsdifftime(&tor, &tot)); } meas->carrier_phase -= internal_data->carrier_phase_offset; diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index a5356aba..eaea26b7 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -51,26 +51,15 @@ */ #define L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS 300 -/* Number of chips to integrate over in the short cycle interval [ms] */ -#define L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS \ - (1e3 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS / GPS_CA_CHIPPING_RATE) - /* Number of chips to integrate over in the long cycle interval [chips] */ #define L2CM_TRACK_LONG_CYCLE_INTERVAL_CHIPS \ (2 * GPS_L2CM_CHIPS_NUM - L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS) -/* Number of chips to integrate over in the long cycle interval [ms] */ -#define L2CM_TRACK_LONG_CYCLE_INTERVAL_MS \ - (L2C_COHERENT_INTEGRATION_TIME_MS - L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS) - -/* Number of chips to integrate over in the long cycle interval [chips] */ +/* Number of chips to integrate over in the long cycle interval + at start-up [chips] */ #define L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_CHIPS \ (2 * GPS_L2CM_CHIPS_NUM - 2 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS) -/* Number of chips to integrate over in the startup long cycle interval [ms] */ -#define L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_MS \ - (L2C_COHERENT_INTEGRATION_TIME_MS - 2 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS) - #define L2CM_TRACK_SETTING_SECTION "l2cm_track" /* code: nbw zeta k carr_to_code @@ -408,13 +397,12 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, /* Read early ([0]), prompt ([1]) and late ([2]) correlations. */ if (data->short_cycle) { - /* The throw away data. Not needed for the short cycle. - And we also do not want to clobber common_data content by - by writing these data to it as it contains valid data for - the previous full 20 ms cycle. */ - u32 sample_count; /* Total num samples channel has tracked for. */ - double code_phase_early; /* Early code phase. */ - double carrier_phase; /* Carrier phase in NAP register units. */ + /* The throw away data. They are not needed for the short cycle. + And we also do not want to clobber common_data content + as it contains valid data for the previous full 20 ms cycle. */ + u32 sample_count; /* Total num samples channel has tracked for. */ + double code_phase_early; /* Early code phase. */ + double carrier_phase; /* Carrier phase in NAP register units. */ tracker_correlations_read(channel_info->context, data->cs, &sample_count, @@ -436,13 +424,6 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, } } - u8 int_ms = data->short_cycle ? - L2CM_TRACK_SHORT_CYCLE_INTERVAL_MS : - L2CM_TRACK_LONG_CYCLE_INTERVAL_MS; - common_data->TOW_ms = tracker_tow_update(channel_info->context, - common_data->TOW_ms, - int_ms); - /* We're doing long integrations, alternate between short and long * cycles. This is because of FPGA pipelining and latency. * The loop parameters can only be updated at the end of the second @@ -461,6 +442,10 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, common_data->update_count += data->int_ms; + common_data->TOW_ms = tracker_tow_update(channel_info->context, + common_data->TOW_ms, + data->int_ms); + /* Call the bit sync update API to do data decoding */ tracker_bit_sync_update(channel_info->context, data->int_ms, data->cs[1].I); From d4f6c0571c6d0a36f37252c5e57d6b1331f51ebf Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Fri, 27 May 2016 10:35:28 +0300 Subject: [PATCH 12/21] Add L2C TOW decoding --- src/Makefile | 4 +- src/board/v3/Makefile.inc | 1 + src/board/v3/platform_signal.c | 2 + src/board/v3/platform_signal.h | 1 + src/decode/decode_gps_l2c.c | 120 +++++++++++++++++++++++++++++++++ src/decode/decode_gps_l2c.h | 19 ++++++ src/manage.c | 4 +- src/track_internal.h | 2 +- 8 files changed, 148 insertions(+), 5 deletions(-) create mode 100644 src/decode/decode_gps_l2c.c create mode 100644 src/decode/decode_gps_l2c.h diff --git a/src/Makefile b/src/Makefile index 9f6f9467..639722d1 100644 --- a/src/Makefile +++ b/src/Makefile @@ -159,6 +159,7 @@ INCDIR = $(STARTUPINC) $(PORTINC) $(KERNINC) \ $(SWIFTNAV_ROOT)/libswiftnav/include \ $(SWIFTNAV_ROOT)/src \ $(SWIFTNAV_ROOT)/src/board \ + $(SWIFTNAV_ROOT)/libswiftnav/libfec/include \ # # Project, sources and paths @@ -216,12 +217,13 @@ DLIBDIR = $(SWIFTNAV_ROOT)/libsbp/c/build/src \ $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/BLAS/SRC \ $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/SRC \ $(SWIFTNAV_ROOT)/libswiftnav/build/clapack-3.2.1-CMAKE/F2CLIBS/libf2c \ + $(SWIFTNAV_ROOT)/libswiftnav/build/libfec/src \ $(SWIFTNAV_ROOT)/libswiftnav/build/src # List all default libraries here DLIBS = -lsbp-static -lswiftnav-static \ -llapack -lcblas -lblas \ - -lf2c -lm -lc -lnosys + -lf2c -lm -lc -lnosys -lfec # # End of default section diff --git a/src/board/v3/Makefile.inc b/src/board/v3/Makefile.inc index 495aa9d7..e0992a56 100644 --- a/src/board/v3/Makefile.inc +++ b/src/board/v3/Makefile.inc @@ -79,6 +79,7 @@ BOARDSRC := \ $(SWIFTNAV_ROOT)/src/track/track_gps_l1ca.o \ $(SWIFTNAV_ROOT)/src/track/track_gps_l2cm.o \ $(SWIFTNAV_ROOT)/src/decode/decode_gps_l1ca.o \ + $(SWIFTNAV_ROOT)/src/decode/decode_gps_l2c.o \ BOARDASM := \ $(BOARDDIR)/cpu_init.s \ diff --git a/src/board/v3/platform_signal.c b/src/board/v3/platform_signal.c index 9b474436..70dd2b6b 100644 --- a/src/board/v3/platform_signal.c +++ b/src/board/v3/platform_signal.c @@ -16,6 +16,7 @@ #include "track/track_gps_l2cm.h" #include "decode/decode_gps_l1ca.h" +#include "decode/decode_gps_l2c.h" void platform_track_setup(void) { @@ -26,4 +27,5 @@ void platform_track_setup(void) void platform_decode_setup(void) { decode_gps_l1ca_register(); + decode_gps_l2c_register(); } diff --git a/src/board/v3/platform_signal.h b/src/board/v3/platform_signal.h index c56af53a..e9b70324 100644 --- a/src/board/v3/platform_signal.h +++ b/src/board/v3/platform_signal.h @@ -28,6 +28,7 @@ /* Decoder configuration */ #define NUM_DECODER_CHANNELS NAP_MAX_N_TRACK_CHANNELS #define NUM_GPS_L1CA_DECODERS NAP_MAX_N_TRACK_CHANNELS +#define NUM_GPS_L2CM_DECODERS NAP_MAX_N_TRACK_CHANNELS void platform_track_setup(void); void platform_decode_setup(void); diff --git a/src/decode/decode_gps_l2c.c b/src/decode/decode_gps_l2c.c new file mode 100644 index 00000000..dc8994bd --- /dev/null +++ b/src/decode/decode_gps_l2c.c @@ -0,0 +1,120 @@ +/* + * 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. + */ + +#include "decode_gps_l2c.h" +#include "decode.h" + +#include +#include +#include /* For BIT_POLARITY_... constants */ +#include +#include + +#include "ephemeris.h" +#include "track.h" +#include "sbp.h" +#include "sbp_utils.h" +#include "signal.h" + +typedef struct { + cnav_msg_t cnav_msg; + cnav_msg_decoder_t cnav_msg_decoder; +} gps_l2c_decoder_data_t; + +static decoder_t gps_l2c_decoders[NUM_GPS_L2CM_DECODERS]; +static gps_l2c_decoder_data_t gps_l2c_decoder_data[NUM_GPS_L2CM_DECODERS]; + +static void decoder_gps_l2c_init(const decoder_channel_info_t *channel_info, + decoder_data_t *decoder_data); +static void decoder_gps_l2c_disable(const decoder_channel_info_t *channel_info, + decoder_data_t *decoder_data); +static void decoder_gps_l2c_process(const decoder_channel_info_t *channel_info, + decoder_data_t *decoder_data); + +static const decoder_interface_t decoder_interface_gps_l2c = { + .code = CODE_GPS_L2CM, + .init = decoder_gps_l2c_init, + .disable = decoder_gps_l2c_disable, + .process = decoder_gps_l2c_process, + .decoders = gps_l2c_decoders, + .num_decoders = NUM_GPS_L2CM_DECODERS +}; + +static decoder_interface_list_element_t list_element_gps_l2c = { + .interface = &decoder_interface_gps_l2c, + .next = 0 +}; + +void decode_gps_l2c_register(void) +{ + for (u32 i=0; icnav_msg_decoder); +} + +static void decoder_gps_l2c_disable(const decoder_channel_info_t *channel_info, + decoder_data_t *decoder_data) +{ + (void)channel_info; + (void)decoder_data; +} + +static void decoder_gps_l2c_process(const decoder_channel_info_t *channel_info, + decoder_data_t *decoder_data) +{ + gps_l2c_decoder_data_t *data = decoder_data; + + /* Process incoming nav bits */ + s8 soft_bit; + while (tracking_channel_nav_bit_get(channel_info->tracking_channel, + &soft_bit)) { + /* Update TOW */ + u8 symbol_probability; + u32 delay; + s32 tow_ms; + + /* Symbol value probability, where 0x00 - 100% of 0, 0xFF - 100% of 1. */ + symbol_probability = soft_bit + 128; + + bool decoded = cnav_msg_decoder_add_symbol(&data->cnav_msg_decoder, + symbol_probability, + &data->cnav_msg, + &delay); + + if (!decoded) { + continue; + } + + tow_ms = data->cnav_msg.tow * GPS_CNAV_MSG_LENGTH * GPS_L2C_SYMBOL_LENGTH; + tow_ms += delay * GPS_L2C_SYMBOL_LENGTH; + + s8 bit_polarity = data->cnav_msg.bit_polarity; + + if ((tow_ms >= 0) && (bit_polarity != BIT_POLARITY_UNKNOWN)) { + if (!tracking_channel_time_sync(channel_info->tracking_channel, tow_ms, + bit_polarity)) { + log_warn_sid(channel_info->sid, "TOW set failed"); + } + } + } +} diff --git a/src/decode/decode_gps_l2c.h b/src/decode/decode_gps_l2c.h new file mode 100644 index 00000000..d42218f9 --- /dev/null +++ b/src/decode/decode_gps_l2c.h @@ -0,0 +1,19 @@ +/* + * 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. + */ +#ifndef SWIFTNAV_DECODE_GPS_L2C_H +#define SWIFTNAV_DECODE_GPS_L2C_H + +#include + +void decode_gps_l2c_register(void); + +#endif /* SWIFTNAV_DECODE_GPS_L2C_H */ diff --git a/src/manage.c b/src/manage.c index c80e0165..0d197b10 100644 --- a/src/manage.c +++ b/src/manage.c @@ -458,9 +458,7 @@ static u8 manage_track_new_acq(gnss_signal_t sid) */ for (u8 i=0; i Date: Thu, 2 Jun 2016 13:38:28 +0300 Subject: [PATCH 13/21] Fix L2C carrier phase initial offset --- src/track.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/track.c b/src/track.c index bd202171..022cb2fb 100644 --- a/src/track.c +++ b/src/track.c @@ -590,7 +590,8 @@ void tracking_channel_measurement_get(tracker_channel_id_t id, u64 ref_tc, /* Adjust carrier phase initial integer offset to be approximately equal to pseudorange. */ if ((time_quality == TIME_FINE) - && (internal_data->carrier_phase_offset == 0.0)) { + && (internal_data->carrier_phase_offset == 0.0) + && (internal_data->bit_polarity != BIT_POLARITY_UNKNOWN)) { gps_time_t tor = rx2gpstime(ref_tc + meas->rec_time_delta); gps_time_t tot; tot.tow = 1e-3 * meas->time_of_week_ms; From 767d0eee94c9993f4c0ffe9025648011b470f323 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Wed, 1 Jun 2016 15:43:21 +0300 Subject: [PATCH 14/21] Replace GPS_L1_LAMBDA by code_to_lambda(code) --- src/solution.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/solution.c b/src/solution.c index e3537547..ad38db50 100644 --- a/src/solution.c +++ b/src/solution.c @@ -654,7 +654,8 @@ static void solution_thread(void *arg) memcpy(nm, &nav_meas_tdcp[i], sizeof(*nm)); } - nm->raw_pseudorange += t_err * nm->raw_doppler * GPS_L1_LAMBDA; + nm->raw_pseudorange += t_err * nm->raw_doppler * + code_to_lambda(nm->sid.code); nm->raw_carrier_phase += t_err * nm->raw_doppler; nm->tot = new_obs_time; From 5898db31611d522cd9db34c29b9c596a56cc277d Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Mon, 16 May 2016 15:07:23 +0300 Subject: [PATCH 15/21] Change from hard coded l2c mask to decoded --- src/track/track_gps_l2cm.c | 33 ++------------------------------- 1 file changed, 2 insertions(+), 31 deletions(-) diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index eaea26b7..ae653f3c 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -26,6 +26,7 @@ #include "track_api.h" #include "decode.h" #include "manage.h" +#include "l2c_capb.h" #include #include @@ -182,35 +183,6 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, double carrier_freq, float cn0_init) { - /* First, get L2C capability for the SV from NDB */ - u32 l2c_cpbl; - // TODO: uncomment this as soon as NDB gets available - // ndb_gps_l2cm_l2c_cap_read(&l2c_cpbl); - - // TODO: remove this as soon as NDB gets available - // GPS PRNs with L2C capability: - // 01 03 05 06 07 08 09 10 12 15 17 24 25 26 27 29 30 31 32 - // as per http://tinyurl.com/zj5q62h - l2c_cpbl = (u32)1 << (1 - 1); - l2c_cpbl |= (u32)1 << (3 - 1); - l2c_cpbl |= (u32)1 << (5 - 1); - l2c_cpbl |= (u32)1 << (6 - 1); - l2c_cpbl |= (u32)1 << (7 - 1); - l2c_cpbl |= (u32)1 << (8 - 1); - l2c_cpbl |= (u32)1 << (9 - 1); - l2c_cpbl |= (u32)1 << (10 - 1); - l2c_cpbl |= (u32)1 << (12 - 1); - l2c_cpbl |= (u32)1 << (15 - 1); - l2c_cpbl |= (u32)1 << (17 - 1); - l2c_cpbl |= (u32)1 << (24 - 1); - l2c_cpbl |= (u32)1 << (25 - 1); - l2c_cpbl |= (u32)1 << (26 - 1); - l2c_cpbl |= (u32)1 << (27 - 1); - l2c_cpbl |= (u32)1 << (29 - 1); - l2c_cpbl |= (u32)1 << (30 - 1); - l2c_cpbl |= (u32)1 << (31 - 1); - l2c_cpbl |= (u32)1 << (32 - 1); - /* compose SID: same SV, but code is L2 CM */ gnss_signal_t sid = construct_sid(CODE_GPS_L2CM, sat); @@ -218,8 +190,7 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, return; /* L2C signal from the SV is already in track */ } - if (0 == (l2c_cpbl & ((u32)1 << (sat - 1)))) { - log_info_sid(sid, "SV does not support L2C signal"); + if (0 == (gps_l2cm_l2c_cap_read() & ((u32)1 << (sat - 1)))) { return; } From 2564d5116da34853bb5a662f9b5d75f7d7578499 Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Tue, 17 May 2016 10:22:15 +0300 Subject: [PATCH 16/21] Fix L2C reacquisition --- src/manage.c | 42 ++++++++++++++++++++++++++++++++++++++ src/manage.h | 3 +++ src/track/track_gps_l1ca.c | 4 +++- src/track/track_gps_l2cm.c | 8 ++++++++ 4 files changed, 56 insertions(+), 1 deletion(-) diff --git a/src/manage.c b/src/manage.c index c80e0165..3848ae31 100644 --- a/src/manage.c +++ b/src/manage.c @@ -81,6 +81,9 @@ static acq_status_t acq_status[PLATFORM_SIGNAL_COUNT]; static bool track_mask[PLATFORM_SIGNAL_COUNT]; +static u32 l1ca_l2cm_handover_mask = 0; +static MUTEX_DECL(handover_mask_mutex); + #define SCORE_COLDSTART 100 #define SCORE_WARMSTART 200 #define SCORE_BELOWMASK 0 @@ -720,6 +723,10 @@ static void manage_tracking_startup(void) acq->dopp_hint_high = startup_params.carrier_freq + ACQ_FULL_CF_STEP; } + /* release handover mask */ + if (CODE_GPS_L2CM == startup_params.sid.code) + l1ca_l2cm_handover_release(startup_params.sid.sat); + continue; } @@ -733,6 +740,11 @@ static void manage_tracking_startup(void) TRACKING_ELEVATION_UNKNOWN)) { log_error("tracker channel init failed"); } + + /* release handover mask */ + if (CODE_GPS_L2CM == startup_params.sid.code) + l1ca_l2cm_handover_release(startup_params.sid.sat); + /* TODO: Initialize elevation from ephemeris if we know it precisely */ /* Start the decoder channel */ @@ -803,4 +815,34 @@ static bool tracking_startup_fifo_read(tracking_startup_fifo_t *fifo, return false; } +/** Set satellite as being handed over. + * + * \param sat satellite prn. + * + * \return true if satellite wasn't under handover process, otherwise false. + */ +bool l1ca_l2cm_handover_reserve(u8 sat) +{ + chMtxLock(&handover_mask_mutex); + if (0 != (l1ca_l2cm_handover_mask & ((u32)1 << (sat - 1)))) { + chMtxUnlock(&handover_mask_mutex); + return false; + } + + l1ca_l2cm_handover_mask |= (u32)1 << (sat - 1); + chMtxUnlock(&handover_mask_mutex); + return true; +} + +/** Release satellite from being handed over. + * + * \param sat satellite prn. + */ +void l1ca_l2cm_handover_release(u8 sat) +{ + chMtxLock(&handover_mask_mutex); + l1ca_l2cm_handover_mask &= ~((u32)1 << (sat - 1)); + chMtxUnlock(&handover_mask_mutex); +} + /** \} */ diff --git a/src/manage.h b/src/manage.h index 225351b7..67fab0f3 100644 --- a/src/manage.h +++ b/src/manage.h @@ -85,4 +85,7 @@ u8 tracking_channels_ready(void); bool tracking_startup_ready(gnss_signal_t sid); bool tracking_startup_request(const tracking_startup_params_t *startup_params); +bool l1ca_l2cm_handover_reserve(u8 sat); +void l1ca_l2cm_handover_release(u8 sat); + #endif diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index af87aa2d..1764c63e 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -344,13 +344,15 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, /* Indicate that a mode change has occurred. */ common_data->mode_change_count = common_data->update_count; + } + if (data->lock_detect.outo && + tracker_bit_aligned(channel_info->context)) do_l1ca_to_l2cm_handover(common_data->sample_count, channel_info->sid.sat, common_data->code_phase_early, common_data->carrier_freq, common_data->cn0); - } u32 chips_to_correlate = (1 == data->int_ms) ? L1CA_TRACK_MS_TO_CHIPS(1) : diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index eaea26b7..548e9f83 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -220,6 +220,14 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, if (0 == (l2c_cpbl & ((u32)1 << (sat - 1)))) { log_info_sid(sid, "SV does not support L2C signal"); + } + + /* Prevent tracking_startup_fifo from being flooded with same satellite. + * After calling tracking_startup_request() it takes few seconds before + * channel is marked as active (after succesful tracker channel init) and + * during this time multiple handover requests arrive. */ + if (!l1ca_l2cm_handover_reserve(sat)) { + /* handover already in progress */ return; } From cd4cb59d25e569c8b70e75e42910c60476a71b76 Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Thu, 26 May 2016 10:39:15 +0300 Subject: [PATCH 17/21] Use L1CA ephes for L2CM --- src/ephemeris.c | 5 +++++ src/track/track_gps_l2cm.c | 1 + 2 files changed, 6 insertions(+) diff --git a/src/ephemeris.c b/src/ephemeris.c index 7a6aa3c9..22cf6c0d 100644 --- a/src/ephemeris.c +++ b/src/ephemeris.c @@ -150,5 +150,10 @@ void ephemeris_unlock(void) ephemeris_t *ephemeris_get(gnss_signal_t sid) { assert(sid_supported(sid)); + + /* L2CM uses L1CA ephes */ + if (CODE_GPS_L2CM == sid.code) + sid.code = CODE_GPS_L1CA; + return &es[sid_to_global_index(sid)]; } diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index 548e9f83..bcadff3f 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -220,6 +220,7 @@ void do_l1ca_to_l2cm_handover(u32 sample_count, if (0 == (l2c_cpbl & ((u32)1 << (sat - 1)))) { log_info_sid(sid, "SV does not support L2C signal"); + return; } /* Prevent tracking_startup_fifo from being flooded with same satellite. From 82bca743931d84b9f535bff25c054f45fd1d2bc8 Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Fri, 3 Jun 2016 10:53:43 +0300 Subject: [PATCH 18/21] Pass MAX_CHANNELS value to libswiftnav according to PIKSI_HW --- Makefile | 2 ++ src/board/v2/nap/nap_hw.h | 3 ++- src/board/v3/nap/nap_hw.h | 3 ++- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index e6e341ea..3993dca8 100644 --- a/Makefile +++ b/Makefile @@ -26,10 +26,12 @@ MAKEFLAGS += PIKSI_HW=$(PIKSI_HW) ifeq ($(PIKSI_HW),v2) CMAKEFLAGS += -DCMAKE_SYSTEM_PROCESSOR=cortex-m4 + CMAKEFLAGS += -DMAX_CHANNELS=11 endif ifeq ($(PIKSI_HW),v3) CMAKEFLAGS += -DCMAKE_SYSTEM_PROCESSOR=cortex-a9 + CMAKEFLAGS += -DMAX_CHANNELS=31 endif .PHONY: all tests firmware docs hitl_setup hitl hitlv3 .FORCE diff --git a/src/board/v2/nap/nap_hw.h b/src/board/v2/nap/nap_hw.h index b2726f54..52005cf8 100644 --- a/src/board/v2/nap/nap_hw.h +++ b/src/board/v2/nap/nap_hw.h @@ -14,6 +14,7 @@ #ifndef SWIFTNAV_NAP_HW_V2_H #define SWIFTNAV_NAP_HW_V2_H +#include #include /** \addtogroup nap @@ -39,7 +40,7 @@ #define NAP_HASH_NOTREADY 2 /** Max number of tracking channels NAP configuration will be built with. */ -#define NAP_MAX_N_TRACK_CHANNELS 12 +#define NAP_MAX_N_TRACK_CHANNELS (MAX_CHANNELS + 1) u8 nap_conf_done(void); u8 nap_hash_rd_done(void); diff --git a/src/board/v3/nap/nap_hw.h b/src/board/v3/nap/nap_hw.h index dd24d6be..0b7e94ec 100644 --- a/src/board/v3/nap/nap_hw.h +++ b/src/board/v3/nap/nap_hw.h @@ -13,10 +13,11 @@ #ifndef SWIFTNAV_NAP_REGS_H #define SWIFTNAV_NAP_REGS_H +#include #include /** Max number of tracking channels NAP configuration will be built with. */ -#define NAP_MAX_N_TRACK_CHANNELS 32 +#define NAP_MAX_N_TRACK_CHANNELS (MAX_CHANNELS + 1) typedef struct { volatile uint32_t STATUS; From ca8f22da61a0f9809206d7207989b1e7b80a1c1d Mon Sep 17 00:00:00 2001 From: Pasi Miettinen Date: Fri, 13 May 2016 14:42:32 +0300 Subject: [PATCH 19/21] Add CN0 filter redesing values for L2C --- src/track/track_gps_l2cm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index eaea26b7..80627488 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -70,7 +70,7 @@ #define LD_PARAMS "0.0247, 1.5, 50, 240" #define LD_PARAMS_DISABLE "0.02, 1e-6, 1, 1" -#define CN0_EST_LPF_CUTOFF 5 +#define CN0_EST_LPF_CUTOFF 0.1f static struct loop_params { float code_bw, code_zeta, code_k, carr_to_code; From 81d3750cebeb7c987f10cdc09b847c252912660a Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Fri, 10 Jun 2016 11:04:49 +0300 Subject: [PATCH 20/21] Update L2C CN0 estimation code to match the new CN0 API --- src/track/track_gps_l1ca.c | 3 ++- src/track/track_gps_l2cm.c | 15 +++++++++++++-- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/track/track_gps_l1ca.c b/src/track/track_gps_l1ca.c index 3ea33be0..93862d76 100644 --- a/src/track/track_gps_l1ca.c +++ b/src/track/track_gps_l1ca.c @@ -296,7 +296,8 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, } common_data->cn0 = cn0_est(&data->cn0_est, pparams, - cs[1].I/data->int_ms, cs[1].Q/data->int_ms); + (float) cs[1].I/data->int_ms, + (float) cs[1].Q/data->int_ms); } if (common_data->cn0 > track_cn0_drop_thres) diff --git a/src/track/track_gps_l2cm.c b/src/track/track_gps_l2cm.c index 26825ecb..216b8320 100644 --- a/src/track/track_gps_l2cm.c +++ b/src/track/track_gps_l2cm.c @@ -432,9 +432,20 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, corr_t* cs = data->cs; /* Update C/N0 estimate */ + + /* Pre-computed C/N0 estimator and filter parameters. The parameters are + * computed using equivalent of cn0_est_compute_params() function for + * integration period of 20ms and cut-off frequency of 0.1 Hz. + */ + static const cn0_est_params_t pre_computed = { + 1.6989700e+01f, 3.9130205e-05f, -1.9822289e+00f, 9.8238545e-01f + }; + common_data->cn0 = cn0_est(&data->cn0_est, - cs[1].I / data->int_ms, - cs[1].Q / data->int_ms); + &pre_computed, + (float) cs[1].I / data->int_ms, + (float) cs[1].Q / data->int_ms); + if (common_data->cn0 > track_cn0_drop_thres) { common_data->cn0_above_drop_thres_count = common_data->update_count; } From 1b2b886df21471f5a7b8bffc4c27424be4113af4 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Tue, 10 May 2016 15:23:20 +0300 Subject: [PATCH 21/21] Add Iono and Tropo correction usage --- src/Makefile | 1 + src/base_obs.c | 16 +++++++++++ src/decode/decode_gps_l1ca.c | 7 +++++ src/iono.c | 53 ++++++++++++++++++++++++++++++++++++ src/iono.h | 22 +++++++++++++++ src/solution.c | 24 +++++++++++++++- 6 files changed, 122 insertions(+), 1 deletion(-) create mode 100644 src/iono.c create mode 100644 src/iono.h diff --git a/src/Makefile b/src/Makefile index 1a7128d6..c6818a9a 100644 --- a/src/Makefile +++ b/src/Makefile @@ -123,6 +123,7 @@ CSRC := $(STARTUPSRC) \ $(SWIFTNAV_ROOT)/src/decode.o \ $(SWIFTNAV_ROOT)/src/signal.o \ $(SWIFTNAV_ROOT)/src/l2c_capb.o \ + $(SWIFTNAV_ROOT)/src/iono.o \ $(SWIFTNAV_ROOT)/src/sid_set.o \ main.c diff --git a/src/base_obs.c b/src/base_obs.c index 729bb995..de3e407f 100644 --- a/src/base_obs.c +++ b/src/base_obs.c @@ -35,6 +35,7 @@ #include "base_obs.h" #include "ephemeris.h" #include "signal.h" +#include "iono.h" extern bool disable_raim; @@ -146,6 +147,21 @@ static void update_obss(obss_t *new_obss) gnss_solution soln; dops_t dops; + /* check if we have fix, if yes, calculate iono and tropo correction */ + if(base_obss.has_pos) { + double llh[3]; + wgsecef2llh(base_obss.pos_ecef, llh); + log_debug("Base: IONO/TROPO correction"); + ionosphere_t i_params; + ionosphere_t *p_i_params = &i_params; + /* get iono parameters if available */ + if(!gps_iono_params_read(p_i_params)) { + p_i_params = NULL; + } + calc_iono_tropo(base_obss.n, base_obss.nm, base_obss.pos_ecef, llh, + p_i_params); + } + /* Calculate a position solution. */ /* disable_raim controlled by external setting (see solution.c). */ s32 ret = calc_PVT(base_obss.n, base_obss.nm, disable_raim, &soln, &dops); diff --git a/src/decode/decode_gps_l1ca.c b/src/decode/decode_gps_l1ca.c index 72e26ace..bfd3dd05 100644 --- a/src/decode/decode_gps_l1ca.c +++ b/src/decode/decode_gps_l1ca.c @@ -23,6 +23,7 @@ #include "sbp_utils.h" #include "signal.h" #include "l2c_capb.h" +#include "iono.h" typedef struct { nav_msg_t nav_msg; @@ -115,6 +116,12 @@ static void decoder_gps_l1ca_process(const decoder_channel_info_t *channel_info, gps_l2cm_l2c_cap_store(dd.gps_l2c_sv_capability); } + if (dd.iono_corr_upd_flag) { + /* store new iono parameters */ + log_debug("Iono parameters received"); + gps_iono_params_store(&dd.iono); + } + if(dd.ephemeris_upd_flag) { /* Decoded a new ephemeris. */ ephemeris_new(&dd.ephemeris); diff --git a/src/iono.c b/src/iono.c new file mode 100644 index 00000000..400fc0b6 --- /dev/null +++ b/src/iono.c @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Dmitry Tatarinov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include +#include + +#include "iono.h" +#include "signal.h" + +MUTEX_DECL(iono_mutex); + +static ionosphere_t iono_params; +static bool iono_params_decoded_flag = false; + +/** Stores ionospheric parameters + * \param params pointer to ionospheric parameters to be stored + */ +void gps_iono_params_store(ionosphere_t *params) +{ + assert(params != NULL); + chMtxLock(&iono_mutex); + memcpy(&iono_params, params, sizeof(ionosphere_t)); + iono_params_decoded_flag = true; + chMtxUnlock(&iono_mutex); +} + +/** Reads ionospheric parameters + * \param params pointer to ionospheric + * \return 1 if iono parameters available otherwise 0 + */ +u8 gps_iono_params_read(ionosphere_t *params) +{ + assert(params != NULL); + chMtxLock(&iono_mutex); + if (iono_params_decoded_flag) { + memcpy(params, &iono_params, sizeof(ionosphere_t)); + chMtxUnlock(&iono_mutex); + return 1; + } else { + chMtxUnlock(&iono_mutex); + return 0; + } +} diff --git a/src/iono.h b/src/iono.h new file mode 100644 index 00000000..0b434849 --- /dev/null +++ b/src/iono.h @@ -0,0 +1,22 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Dmitry Tatarinov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef SRC_IONO_H_ +#define SRC_IONO_H_ + +#include +#include + +void gps_iono_params_store(ionosphere_t *params); +u8 gps_iono_params_read(ionosphere_t *params); + +#endif /* SRC_IONO_H_ */ diff --git a/src/solution.c b/src/solution.c index ed467f47..42b3b12d 100644 --- a/src/solution.c +++ b/src/solution.c @@ -9,7 +9,6 @@ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. */ - #include #include @@ -23,6 +22,7 @@ #include #include #include +#include #define memory_pool_t MemoryPool #include @@ -43,6 +43,7 @@ #include "signal.h" #include "system_monitor.h" #include "main.h" +#include "iono.h" #include "sid_set.h" /* Maximum CPU time the solution thread is allowed to use. */ @@ -433,6 +434,9 @@ static void sol_thd_sleep(systime_t *deadline, systime_t interval) static THD_WORKING_AREA(wa_solution_thread, 8000); static void solution_thread(void *arg) { + /* The flag is true when we have a fix */ + bool soln_flag = false; + (void)arg; chRegSetThreadName("solution"); @@ -534,6 +538,20 @@ static void solution_thread(void *arg) continue; } + /* check if we have a solution, if yes calc iono and tropo correction */ + if (soln_flag) { + ionosphere_t i_params; + ionosphere_t *p_i_params = &i_params; + /* get iono parameters if available */ + if(!gps_iono_params_read(p_i_params)) { + p_i_params = NULL; + } + calc_iono_tropo(n_ready_tdcp, nav_meas_tdcp, + position_solution.pos_ecef, + position_solution.pos_llh, + p_i_params); + } + dops_t dops; /* Calculate the SPP position * disable_raim controlled by external setting. Defaults to false. */ @@ -548,11 +566,15 @@ static void solution_thread(void *arg) log_warn("PVT solver: %s (code %d)", pvt_err_msg[-pvt_ret-1], pvt_ret); ); + soln_flag = false; + /* Send just the DOPs and exit the loop */ solution_send_sbp(0, &dops, clock_jump); continue; } + soln_flag = true; + if (pvt_ret == 1) log_warn("calc_PVT: RAIM repair");