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/libswiftnav b/libswiftnav index f82b1f42..51bafb45 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit f82b1f42224dd9cd8dabd037907c731f1b94a867 +Subproject commit 51bafb452a09bf497b1a40f331642b4cafc8ef6f diff --git a/src/Makefile b/src/Makefile index 9f6f9467..c6818a9a 100644 --- a/src/Makefile +++ b/src/Makefile @@ -123,6 +123,8 @@ 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 # C++ sources that can be compiled in ARM or THUMB mode depending on the global @@ -159,6 +161,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 +219,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/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/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/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/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/Makefile.inc b/src/board/v3/Makefile.inc index 9a669640..e0992a56 100644 --- a/src/board/v3/Makefile.inc +++ b/src/board/v3/Makefile.inc @@ -77,7 +77,9 @@ 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 \ + $(SWIFTNAV_ROOT)/src/decode/decode_gps_l2c.o \ BOARDASM := \ $(BOARDDIR)/cpu_init.s \ 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..352fdea1 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; @@ -116,6 +117,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_CODE_GPS_L1CA_SBAS_L1CA = 0, /* GPS L1C/A, SBAS L1CA */ + NAP_CODE_GPS_L2CM /* 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..8730a134 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 @@ -54,40 +55,105 @@ 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; } +/** 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_CODE_GPS_L1CA_SBAS_L1CA; + break; + case CODE_GPS_L2CM: + ret = NAP_CODE_GPS_L2CM; + 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 +171,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_count(sid.code) - cp) * + (1.0 + carrier_freq / code_to_carr_freq(sid.code)); NAP->TRK_TIMING_COMPARE = tc_req; chSysUnlock(); @@ -130,7 +197,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 +208,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..70dd2b6b 100644 --- a/src/board/v3/platform_signal.c +++ b/src/board/v3/platform_signal.c @@ -13,15 +13,19 @@ #include "platform_signal.h" #include "track/track_gps_l1ca.h" +#include "track/track_gps_l2cm.h" #include "decode/decode_gps_l1ca.h" +#include "decode/decode_gps_l2c.h" void platform_track_setup(void) { track_gps_l1ca_register(); + track_gps_l2cm_register(); } 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 1932c94e..e9b70324 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 @@ -23,10 +23,12 @@ /* 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 #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_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/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/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/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/manage.c b/src/manage.c index 6f2646cb..44efa038 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" @@ -80,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 @@ -306,6 +310,10 @@ 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. @@ -675,6 +701,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) { @@ -690,6 +721,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; } @@ -698,10 +733,16 @@ static void manage_tracking_startup(void) startup_params.sample_count, startup_params.code_phase, startup_params.carrier_freq, + startup_params.chips_to_correlate, startup_params.cn0_init, 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 */ @@ -772,4 +813,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 499cea21..67fab0f3 100644 --- a/src/manage.h +++ b/src/manage.h @@ -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 @@ -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; @@ -81,6 +82,10 @@ 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); +bool l1ca_l2cm_handover_reserve(u8 sat); +void l1ca_l2cm_handover_release(u8 sat); + #endif 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/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/solution.c b/src/solution.c index e3537547..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,8 @@ #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. */ #define SOLN_THD_CPU_MAX (0.60f) @@ -432,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"); @@ -462,7 +467,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; iraw_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; diff --git a/src/track.c b/src/track.c index f4f43cdc..022cb2fb 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_count(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; } @@ -584,13 +590,14 @@ 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; - 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; @@ -993,9 +1000,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 +1012,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..93862d76 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 @@ -48,7 +51,10 @@ #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.1f + +/* 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; @@ -61,7 +67,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; @@ -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; } } @@ -238,7 +245,61 @@ 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, + (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; @@ -339,9 +400,20 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, 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) : + 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 new file mode 100644 index 00000000..216b8320 --- /dev/null +++ b/src/track/track_gps_l2cm.c @@ -0,0 +1,590 @@ +/* + * 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" + +/* skip weak attributes for L2C API implementation */ +#define TRACK_GPS_L2CM_INTERNAL +#include "track_gps_l2cm.h" + +#include "track_api.h" +#include "decode.h" +#include "manage.h" +#include "l2c_capb.h" + +#include +#include +#include +#include + +#include +#include +#include "math.h" + +#include "settings.h" +#include "signal.h" + +/** 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 300 + +/* 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 + at start-up [chips] */ +#define L2CM_TRACK_LONG_STARTUP_CYCLE_INTERVAL_CHIPS \ + (2 * GPS_L2CM_CHIPS_NUM - 2 * L2CM_TRACK_SHORT_CYCLE_INTERVAL_CHIPS) + +#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 0.1f + +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. */ +} 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); +} + +/** 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 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, + float code_phase, + double carrier_freq, + float cn0_init) +{ + /* compose SID: same SV, but code is L2 CM */ + 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 == (gps_l2cm_l2c_cap_read() & ((u32)1 << (sat - 1)))) { + return; + } + + /* 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; + } + + 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); + } + + /* 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, + .sample_count = sample_count, + /* 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 + }; + + if (tracking_startup_request(&startup_params)) { + log_info_sid(sid, "L2 CM handover done"); + } else { + log_warn_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); + + /* 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, + 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; + } + if (data->startup) { + data->startup--; + } + return; + } + + /* Read early ([0]), prompt ([1]) and late ([2]) correlations. */ + if (data->short_cycle) { + /* 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, + &code_phase_early, + &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; + } + } + + /* 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; + + 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); + + 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, + &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; + } + + 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 new file mode 100644 index 00000000..40fdba62 --- /dev/null +++ b/src/track/track_gps_l2cm.h @@ -0,0 +1,36 @@ +/* + * 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 + +#ifdef TRACK_GPS_L2CM_INTERNAL +# define L2C_WEAK +#else +# define L2C_WEAK __attribute__ ((weak, alias ("l2c_not_implemented"))) +#endif /* TRACK_GPS_L2CM_INTERNAL */ + +int l2c_not_implemented() __attribute__ ((weak)); +inline int l2c_not_implemented() { return -1; } + +/* 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, + double carrier_freq, + float cn0_init) L2C_WEAK; + +#endif /* SWIFTNAV_TRACK_GPS_L2CM_H */ diff --git a/src/track_api.c b/src/track_api.c index cd3a010f..e43714d6 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. @@ -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. diff --git a/src/track_api.h b/src/track_api.h index 5b41ad48..39b5439b 100644 --- a/src/track_api.h +++ b/src/track_api.h @@ -104,9 +104,10 @@ 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_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); diff --git a/src/track_internal.h b/src/track_internal.h index 8fc5d3d4..b12a4b04 100644 --- a/src/track_internal.h +++ b/src/track_internal.h @@ -21,7 +21,7 @@ /** \addtogroup tracking * \{ */ -#define NAV_BIT_FIFO_SIZE 32 /**< Size of nav bit FIFO. Must be a power of 2 */ +#define NAV_BIT_FIFO_SIZE 64 /**< Size of nav bit FIFO. Must be a power of 2 */ #define NAV_BIT_FIFO_INDEX_MASK ((NAV_BIT_FIFO_SIZE) - 1) #define NAV_BIT_FIFO_INDEX_DIFF(write_index, read_index) \