From 67505f283c1ffd5774e0fc9feb80344d374eea31 Mon Sep 17 00:00:00 2001 From: Adel Mamin Date: Tue, 26 Apr 2016 14:07:13 +0300 Subject: [PATCH] Add L2C tracking (WIP) --- libswiftnav | 2 +- src/Makefile | 1 + src/board/nap/track_channel.h | 4 +- src/board/v3/nap/nap_regs.h | 9 + src/board/v3/nap/nap_track.c | 137 +++++++++-- src/decode.c | 2 +- src/main.c | 4 + src/manage.c | 27 ++- src/manage.h | 3 + src/signal.c | 8 +- src/signal.h | 2 +- src/simulator.c | 3 +- src/solution.c | 2 +- src/track.c | 25 +- src/track.h | 10 +- src/track_api.c | 3 +- src/track_gps_l1ca.c | 3 + src/track_gps_l2cm.c | 414 ++++++++++++++++++++++++++++++++++ src/track_gps_l2cm.h | 20 ++ 19 files changed, 646 insertions(+), 33 deletions(-) create mode 100644 src/track_gps_l2cm.c create mode 100644 src/track_gps_l2cm.h diff --git a/libswiftnav b/libswiftnav index e92adb0f..62da7d77 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit e92adb0f557b14d4763056db7299e84e77c43250 +Subproject commit 62da7d77efaa369523bb8c5bce14d74fe734f26f diff --git a/src/Makefile b/src/Makefile index 633e4862..6a507ceb 100644 --- a/src/Makefile +++ b/src/Makefile @@ -98,6 +98,7 @@ CSRC := $(STARTUPSRC) \ $(SWIFTNAV_ROOT)/src/track_internal.o \ $(SWIFTNAV_ROOT)/src/track_api.o \ $(SWIFTNAV_ROOT)/src/track_gps_l1ca.o \ + $(SWIFTNAV_ROOT)/src/track_gps_l2cm.o \ $(SWIFTNAV_ROOT)/src/manage.o \ $(SWIFTNAV_ROOT)/src/settings.o \ $(SWIFTNAV_ROOT)/src/timing.o \ diff --git a/src/board/nap/track_channel.h b/src/board/nap/track_channel.h index c30648ce..e7c39bc6 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 * @@ -29,7 +29,7 @@ extern u8 nap_track_n_channels; u32 nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, float carrier_freq, float code_phase); -void nap_track_update_wr_blocking(u8 channel, s32 carrier_freq, +void nap_track_update_wr_blocking(code_t code, u8 channel, s32 carrier_freq, u32 code_phase_rate, u8 rollover_count, u8 corr_spacing); void nap_track_corr_rd_blocking(u8 channel, diff --git a/src/board/v3/nap/nap_regs.h b/src/board/v3/nap/nap_regs.h index 80456d2f..c983e0db 100644 --- a/src/board/v3/nap/nap_regs.h +++ b/src/board/v3/nap/nap_regs.h @@ -99,6 +99,15 @@ typedef struct { #define NAP_TRK_CONTROL_PRN_Pos (3u) #define NAP_TRK_CONTROL_PRN_Msk (0x1FU << NAP_TRK_CONTROL_PRN_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) /* Instances */ diff --git a/src/board/v3/nap/nap_track.c b/src/board/v3/nap/nap_track.c index 2321a3f0..b968ffcc 100644 --- a/src/board/v3/nap/nap_track.c +++ b/src/board/v3/nap/nap_track.c @@ -28,58 +28,160 @@ struct { * \param carrier_freq Current carrier frequency (i.e. Doppler) in Hz used for * carrier aiding. * \param n_samples N, the number of samples to propagate for. + * \param code Code identifier. * * \return The propagated code phase in chips. */ -static double propagate_code_phase(double code_phase, double carrier_freq, u32 n_samples) +static 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)) * + GPS_CA_CHIPPING_RATE; code_phase += n_samples * code_phase_rate / SAMPLE_FREQ; 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; } -static u32 calc_length_samples(u8 codes, s32 cp_start, u32 cp_rate) +static u32 calc_length_samples(code_t code_id, u8 codes, s32 cp_start, u32 cp_rate) { - u16 chips = codes * 1023; + u16 chips = codes * code_to_chip_num(code_id); u64 cp_units = (1ULL << NAP_TRACK_CODE_PHASE_FRACTIONAL_WIDTH) * chips - cp_start; u32 samples = cp_units / cp_rate; return samples; } +u16 sid_to_init_g1(gnss_signal_t sid) +{ + u16 ret; + 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 = (u16) gps_l2cm_prns_init_values[sid.sat]; + break; + + case CODE_GPS_L1CA: + default: + ret = 0x3ff; + break; + } + + return ret; +} + +u8 sid_to_rf_fronend_channel(gnss_signal_t sid) +{ + u8 ret; + switch (sid.code) { + case CODE_GPS_L1CA: + case CODE_SBAS_L1CA: + ret = 0; + break; + case CODE_GPS_L2CM: + ret = 3; + break; + default: + assert(0); + break; + } + return ret; +} + u32 nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, float carrier_freq, float code_phase) { u32 now = NAP->TIMING_COUNT; u32 track_count = now + 200000; + /* log_warn("---- ADEL debug %s:%d chan = %d prn = %d, carr_freq = %f, code_phase = %f", */ + /* __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase); */ double 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) */ - track_count += (SAMPLE_FREQ/GPS_CA_CHIPPING_RATE) * (1023.0-cp) * - (1.0 + carrier_freq / GPS_L1_HZ); + track_count += (SAMPLE_FREQ / GPS_CA_CHIPPING_RATE) * + (code_to_chip_num(sid.code) - cp) * + (1.0 + carrier_freq / code_to_carr_freq(sid.code)); + + /* log_warn("---- ADEL debug %s:%d chan = %d prn = %d, carr_freq = %f, code_phase = %f", */ + /* __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase); */ u8 prn = sid.sat - 1; - NAP->TRK_CH[channel].CONTROL = prn << NAP_TRK_CONTROL_PRN_Pos; + u16 control = (prn << NAP_TRK_CONTROL_PRN_Pos) & NAP_TRK_CONTROL_PRN_Msk; + control |= (sid_to_rf_fronend_channel(sid) << NAP_TRK_CONTROL_RF_FE_Pos) & \ + NAP_TRK_CONTROL_RF_FE_Msk; + //control |= (1 << NAP_TRK_CONTROL_RF_FE_CH_Pos) & NAP_TRK_CONTROL_RF_FE_CH_Msk; + + log_warn("- ADEL debug %s:%d ch=%d prn=%d, cf=%f, cp=%f, ctrl=0x%04X", + __FILE__, __LINE__, (int)channel, (int)(sid.sat - 1), carrier_freq, code_phase, control); + + NAP->TRK_CH[channel].CONTROL = control; + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); /* We always start at zero code phase */ NAP->TRK_CH[channel].CODE_INIT_INT = 0; + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); NAP->TRK_CH[channel].CODE_INIT_FRAC = 0; - NAP->TRK_CH[channel].CODE_INIT_G1 = 0x3ff; + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); + NAP->TRK_CH[channel].CODE_INIT_G1 = sid_to_init_g1(sid); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); NAP->TRK_CH[channel].CODE_INIT_G2 = 0x3ff; + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); NAP->TRK_CH[channel].SPACING = (SPACING_HALF_CHIP << 16) | SPACING_HALF_CHIP; - u32 cp_rate = (1.0 + carrier_freq/GPS_L1_HZ) * GPS_CA_CHIPPING_RATE * - NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; + u32 cp_rate = (1.0 + carrier_freq / code_to_carr_freq(sid.code)) * + GPS_CA_CHIPPING_RATE * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; NAP->TRK_CH[channel].CARR_PINC = -carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ; NAP->TRK_CH[channel].CODE_PINC = cp_rate; - pipeline[channel].len = NAP->TRK_CH[channel].LENGTH = calc_length_samples(1, 0, cp_rate)+1; + + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); + + pipeline[channel].len = NAP->TRK_CH[channel].LENGTH = + calc_length_samples(sid.code, 1, 0, cp_rate) + 1; pipeline[channel].code_phase = (NAP->TRK_CH[channel].LENGTH) * cp_rate; pipeline[channel].carrier_phase = NAP->TRK_CH[channel].LENGTH * (s64)-NAP->TRK_CH[channel].CARR_PINC; @@ -87,13 +189,18 @@ u32 nap_track_init(u8 channel, gnss_signal_t sid, u32 ref_timing_count, NAP->TRK_TIMING_COMPARE = track_count - SAMPLE_FREQ / GPS_CA_CHIPPING_RATE; + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); + //chThdSleep(CH_CFG_ST_FREQUENCY * ceil((float)(track_count - now)/SAMPLE_FREQ)); /* Spin until we're really running */ while (!(NAP->TRK_CH[channel].STATUS & NAP_TRK_STATUS_RUNNING)); + + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); + return NAP->TRK_CH[channel].START_SNAPSHOT; } -void nap_track_update_wr_blocking(u8 channel, s32 carrier_freq, +void nap_track_update_wr_blocking(code_t code, u8 channel, s32 carrier_freq, u32 code_phase_rate, u8 rollover_count, u8 corr_spacing) { @@ -101,7 +208,7 @@ void nap_track_update_wr_blocking(u8 channel, s32 carrier_freq, NAP->TRK_CH[channel].CARR_PINC = -carrier_freq; NAP->TRK_CH[channel].CODE_PINC = code_phase_rate; - NAP->TRK_CH[channel].LENGTH = calc_length_samples(rollover_count + 1, + NAP->TRK_CH[channel].LENGTH = calc_length_samples(code, rollover_count + 1, pipeline[channel].code_phase, code_phase_rate); diff --git a/src/decode.c b/src/decode.c index 6e07b26b..d50e5355 100644 --- a/src/decode.c +++ b/src/decode.c @@ -56,7 +56,7 @@ * are allocated independently when initializing decoding. */ -#define NUM_DECODER_CHANNELS 12 +#define NUM_DECODER_CHANNELS 32 typedef enum { DECODER_CHANNEL_STATE_DISABLED, diff --git a/src/main.c b/src/main.c index f0f83abc..a5658c6e 100644 --- a/src/main.c +++ b/src/main.c @@ -27,6 +27,7 @@ #include "manage.h" #include "track.h" #include "track_gps_l1ca.h" +#include "track_gps_l2cm.h" #include "timing.h" #include "ext_events.h" #include "solution.h" @@ -44,6 +45,8 @@ extern void ext_setup(void); +void track_gps_l2cm_register(void) TRK_WEAK; + /** Compare version strings. * Compares a version of the form 'vX.Y-Z-'. If the first character of the * version is not 'v' then that string will be considered older than any @@ -174,6 +177,7 @@ int main(void) position_setup(); track_setup(); track_gps_l1ca_register(); + track_gps_l2cm_register(); decode_setup(); decode_gps_l1_register(); diff --git a/src/manage.c b/src/manage.c index 740681b0..0232385f 100644 --- a/src/manage.c +++ b/src/manage.c @@ -193,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; @@ -260,8 +267,9 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t, /* sat_pos now holds unit vector from us to satellite */ vector_subtract(3, sat_vel, position_solution.vel_ecef, sat_vel); /* sat_vel now holds velocity of sat relative to us */ - dopp_hint = -GPS_L1_HZ * (vector_dot(3, sat_pos, sat_vel) / GPS_C - + position_solution.clock_bias); + dopp_hint = -code_to_carr_freq(sid.code) * + (vector_dot(3, sat_pos, sat_vel) / GPS_C + + position_solution.clock_bias); /* TODO: Check sign of receiver frequency offset correction */ if (time_quality >= TIME_FINE) dopp_uncertainty = DOPP_UNCERT_EPHEM; @@ -646,6 +654,16 @@ u8 tracking_channels_ready() return n_ready; } +void tracking_lock(void) +{ + chMtxTryLock(&tracking_startup_mutex); +} + +void tracking_unlock(void) +{ + chMtxUnlock(&tracking_startup_mutex); +} + /** Queue a request to start up tracking and decoding for the specified sid. * * \note This function is thread-safe and non-blocking. @@ -696,6 +714,8 @@ static void manage_tracking_startup(void) continue; } + tracking_lock(); + /* Start the tracking channel */ if(!tracker_channel_init(chan, startup_params.sid, startup_params.sample_count, @@ -705,6 +725,9 @@ static void manage_tracking_startup(void) TRACKING_ELEVATION_UNKNOWN)) { log_error("tracker channel init failed"); } + + tracking_unlock(); + /* TODO: Initialize elevation from ephemeris if we know it precisely */ /* Start the decoder channel */ diff --git a/src/manage.h b/src/manage.h index 499cea21..06760a1c 100644 --- a/src/manage.h +++ b/src/manage.h @@ -83,4 +83,7 @@ u8 tracking_channels_ready(void); bool tracking_startup_request(const tracking_startup_params_t *startup_params); +void tracking_lock(void); +void tracking_unlock(void); + #endif diff --git a/src/signal.c b/src/signal.c index 460f73b2..0349bd7e 100644 --- a/src/signal.c +++ b/src/signal.c @@ -172,12 +172,16 @@ bool sid_supported(gnss_signal_t sid) bool code_supported(enum code code) { /* Verify general validity */ - if (!code_valid(code)) + if (!code_valid(code)) { + log_warn("---- ADEL debug %s:%d. code: %d", __FILE__, __LINE__, (int)code); return false; + } /* Verify that the code is supported on this platform */ - if (code_signal_counts[code] == 0) + if (code_signal_counts[code] == 0) { + log_warn("---- ADEL debug %s:%d. code: %d", __FILE__, __LINE__, (int)code); return false; + } return true; } diff --git a/src/signal.h b/src/signal.h index dd4b9413..250ce060 100644 --- a/src/signal.h +++ b/src/signal.h @@ -19,7 +19,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/simulator.c b/src/simulator.c index 17fbffe8..d47b0656 100644 --- a/src/simulator.c +++ b/src/simulator.c @@ -346,7 +346,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->carrier_phase = dist / (GPS_C / GPS_L1_HZ); + nav_meas->carrier_phase = dist / (GPS_C / + code_to_carr_freq(simulation_almanacs[almanac_i].sid.code)); nav_meas->carrier_phase += simulation_fake_carrier_bias[almanac_i]; nav_meas->carrier_phase += rand_gaussian(sim_settings.phase_sigma * sim_settings.phase_sigma); diff --git a/src/solution.c b/src/solution.c index 820dfad8..4ae1c2d3 100644 --- a/src/solution.c +++ b/src/solution.c @@ -513,7 +513,7 @@ static void solution_thread(void *arg) /* Propagate observation to desired time. */ for (u8 i=0; iinfo.sid = sid; tracker_channel->info.context = tracker_channel; @@ -316,21 +320,30 @@ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, /* TODO : change hardcoded sample rate */ common_data_init(&tracker_channel->common_data, ref_sample_count, - carrier_freq, cn0_init); + carrier_freq, cn0_init, sid.code); + + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); + internal_data_init(&tracker_channel->internal_data, sid); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); interface_function(tracker_channel, tracker_interface->init); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); /* Clear error flags before starting NAP tracking channel */ error_flags_clear(tracker_channel); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); /* Change the channel state to ENABLED. */ event(tracker_channel, EVENT_ENABLE); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); } tracker_channel_unlock(tracker_channel); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); u32 snapshot = nap_track_init(tracker_channel->info.nap_channel, sid, ref_sample_count, carrier_freq, code_phase); + log_warn("---- ADEL debug %s:%d", __FILE__, __LINE__); tracker_channel->common_data.sample_count = snapshot; return true; @@ -963,9 +976,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)); @@ -973,8 +988,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 2aea382a..1e75dac4 100644 --- a/src/track.h +++ b/src/track.h @@ -33,11 +33,19 @@ typedef u8 tracker_channel_id_t; /** \} */ +#define TRK_WEAK __attribute__ ((weak, alias ("trk_not_implemented"))) +void trk_not_implemented() __attribute__ ((weak)); +inline void trk_not_implemented() +{ + return; +} + void track_setup(void); void tracking_send_state(void); -float propagate_code_phase(float code_phase, float carrier_freq, u32 n_samples); +double propagate_code_phase(float code_phase, float carrier_freq, + u32 n_samples, code_t code); /* Update interface */ void tracking_channels_update(u32 channels_mask); diff --git a/src/track_api.c b/src/track_api.c index 672d75bb..22cbb1f8 100644 --- a/src/track_api.c +++ b/src/track_api.c @@ -82,7 +82,8 @@ void tracker_retune(tracker_context_t *context, s32 carrier_freq_fp, tracker_internal_context_resolve(context, &channel_info, &internal_data); /* Write NAP UPDATE register. */ - nap_track_update_wr_blocking(channel_info->nap_channel, carrier_freq_fp, + nap_track_update_wr_blocking(channel_info->sid.code, + channel_info->nap_channel, carrier_freq_fp, code_phase_rate_fp, rollover_count, 0); } diff --git a/src/track_gps_l1ca.c b/src/track_gps_l1ca.c index aff670b8..bb3a6e48 100644 --- a/src/track_gps_l1ca.c +++ b/src/track_gps_l1ca.c @@ -9,9 +9,12 @@ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. */ + #define DEBUG 1 #include "track_gps_l1ca.h" #include "track_api.h" +#include "track.h" +#include "decode.h" #include #include diff --git a/src/track_gps_l2cm.c b/src/track_gps_l2cm.c new file mode 100644 index 00000000..6ba25a9e --- /dev/null +++ b/src/track_gps_l2cm.c @@ -0,0 +1,414 @@ +/* + * 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. + */ + +#include "track.h" +#include "track_gps_l2cm.h" +#include "track_api.h" +#include "decode.h" +#include "manage.h" + +#include +#include +#include +#include + +#include + +#include "settings.h" + +#define NUM_GPS_L2CM_TRACKERS 12 + +/** L2C coherent integration time [ms] */ +#define L2C_COHERENT_INTEGRATION_TIME_MS 20 + +/* Alias detection interval [ms] */ +#define L2C_ALIAS_DETECT_INTERVAL_MS 500 + +#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. */ + u32 code_phase_rate_fp; /**< Code phase rate in NAP register units. */ + s32 carrier_freq_fp; /**< Carrier frequency in NAP register units. */ + u32 corr_sample_count; /**< Number of samples in correlation period. */ + 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 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); +} + +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; + + 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->code_phase_rate_fp = common_data->code_phase_rate * + NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; + data->carrier_freq_fp = (s32)(common_data->carrier_freq * + NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ); + data->short_cycle = true; + + /* 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); + +} + +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; +} + +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; + + /* Read early ([0]), prompt ([1]) and late ([2]) correlations. */ + if (data->short_cycle) { + tracker_correlations_read(channel_info->context, data->cs, + &data->corr_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, + &data->corr_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 ? 1 : (data->int_ms - 1); + 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, data->carrier_freq_fp, + data->code_phase_rate_fp, 0); + 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; + + data->code_phase_rate_fp = common_data->code_phase_rate * + NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; + + data->carrier_freq_fp = common_data->carrier_freq * + NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ; + + /* 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; + } + } + + /* Must have (at least optimistic) phase lock */ + /* Must have nav bit sync, and be correctly aligned */ + if ((data->lock_detect.outo) && + tracker_bit_aligned(channel_info->context)) { + log_info_sid(channel_info->sid, "synced @ %u ms, %.1f dBHz", + (unsigned int)common_data->update_count, + common_data->cn0); + /* Indicate that a mode change has occurred. */ + common_data->mode_change_count = common_data->update_count; + } + + tracker_retune(channel_info->context, data->carrier_freq_fp, + data->code_phase_rate_fp, + data->int_ms); +} + +/** 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_gps_l2cm.h b/src/track_gps_l2cm.h new file mode 100644 index 00000000..40b57046 --- /dev/null +++ b/src/track_gps_l2cm.h @@ -0,0 +1,20 @@ +/* + * 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 track_gps_l2cm_register(void); + +#endif