From e8497567b655133e546d72758af835b04f2a2149 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Wed, 2 Mar 2016 15:31:58 +0200 Subject: [PATCH 1/5] Add L2CM tracking functionality L2CM tracking using new framework. --- src/Makefile | 1 + src/main.c | 2 + src/track_gps_l2cm.c | 346 +++++++++++++++++++++++++++++++++++++++++++ src/track_gps_l2cm.h | 19 +++ 4 files changed, 368 insertions(+) create mode 100644 src/track_gps_l2cm.c create mode 100644 src/track_gps_l2cm.h diff --git a/src/Makefile b/src/Makefile index 17b4e43d..eeea4ecc 100644 --- a/src/Makefile +++ b/src/Makefile @@ -117,6 +117,7 @@ CSRC := $(PORTSRC) \ $(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/acq.o \ $(SWIFTNAV_ROOT)/src/manage.o \ $(SWIFTNAV_ROOT)/src/settings.o \ diff --git a/src/main.c b/src/main.c index e16a4106..5e80a5be 100644 --- a/src/main.c +++ b/src/main.c @@ -29,6 +29,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" @@ -186,6 +187,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/track_gps_l2cm.c b/src/track_gps_l2cm.c new file mode 100644 index 00000000..9ae247ae --- /dev/null +++ b/src/track_gps_l2cm.c @@ -0,0 +1,346 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Jacob McNamee + * + * 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_gps_l2cm.h" +#include "track.h" + +#include +#include +#include +#include + +#include + +#include "settings.h" + +#define NUM_GPS_L2CM_TRACKERS 12 //TODO: to be updated + +/* code: nbw zeta k carr_to_code + carrier: nbw zeta k fll_aid */ + +#define GPS_L2CM_LOOP_PARAMS_MED \ + "(20 ms, (1, 0.7, 1, 1200), (13, 0.7, 1, 5))" + +/* k1, k2, lp, lo */ +#define LD_PARAMS_PESS "0.10, 1.4, 200, 50" +#define LD_PARAMS_NORMAL "0.05, 1.4, 150, 50" +#define LD_PARAMS_OPT "0.02, 1.1, 150, 50" +#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 + +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; + +static char loop_params_string[] = GPS_L2CM_LOOP_PARAMS_MED; +static char lock_detect_params_string[] = LD_PARAMS_DISABLE; +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. */ + u32 code_phase_rate_fp_prev; /**< Previous code phase rate in NAP register units. */ + s32 carrier_freq_fp; /**< Carrier frequency in NAP register units. */ + s32 carrier_freq_fp_prev; /**< Previous 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. */ + 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, + const tracker_common_data_t *common_data, + tracker_data_t *tracker_data); +static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, + const tracker_common_data_t *common_data, + tracker_data_t *tracker_data); +static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, + const tracker_common_data_t *common_data, + tracker_data_t *tracker_data); + +static bool parse_loop_params_l2cm(struct setting *s, const char *val); +static bool parse_lock_detect_params_l2cm(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 +}; + +void track_gps_l2cm_register(void) +{ + SETTING_NOTIFY("track", "loop_params", loop_params_string, + TYPE_STRING, parse_loop_params_l2cm); + SETTING_NOTIFY("track", "lock_detect_params", lock_detect_params_string, + TYPE_STRING, parse_lock_detect_params_l2cm); + SETTING("track", "cn0_use", track_cn0_use_thres, TYPE_FLOAT); + SETTING("track", "cn0_drop", track_cn0_drop_thres, TYPE_FLOAT); + SETTING("track", "alias_detect", use_alias_detection, TYPE_BOOL); + + + for (u32 i=0; icontext); + + 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); + + /*DT: I think NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ for L2C should be + * NAP_TRACK_NOMINAL_CODE_PHASE_RATE / 10.23e6, does HW team provide + * file (nap/track_channel.h) with these constants */ + data->code_phase_rate_fp = common_data->code_phase_rate*NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; + data->code_phase_rate_fp_prev = data->code_phase_rate_fp; + data->carrier_freq_fp = (s32)(common_data->carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ); + data->carrier_freq_fp_prev = data->carrier_freq_fp; + + /* 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); + + lock_detect_init(&data->lock_detect, + lock_detect_params.k1, lock_detect_params.k2, + lock_detect_params.lp, lock_detect_params.lo); +} + +static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, + const 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, + const tracker_common_data_t *common_data, + tracker_data_t *tracker_data) +{ + + gps_l2cm_tracker_data_t *data = tracker_data; + + + char buf[SID_STR_LEN_MAX]; + sid_to_string(buf, sizeof(buf), channel_info->sid); + + tracker_channel_correlations_read(channel_info->context, data->cs, + &data->corr_sample_count); + alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); + + + tracker_common_data_t updated_common_data = *common_data; + + + updated_common_data.sample_count += data->corr_sample_count; + updated_common_data.code_phase_early = (u64)updated_common_data.code_phase_early + + (u64)data->corr_sample_count + * data->code_phase_rate_fp_prev; + updated_common_data.carrier_phase += (s64)data->carrier_freq_fp_prev + * data->corr_sample_count; + data->code_phase_rate_fp_prev = data->code_phase_rate_fp; + data->carrier_freq_fp_prev = data->carrier_freq_fp; + + u8 int_ms = data->int_ms; + updated_common_data.TOW_ms = tracker_tow_update(channel_info->context, + updated_common_data.TOW_ms, + int_ms); + + updated_common_data.update_count += data->int_ms; + + tracker_bit_sync_update(channel_info->context, int_ms, data->cs[1].I); + + /* Correlations should already be in chan->cs thanks to + * tracking_channel_get_corrs. */ + corr_t* cs = data->cs; + + /* Update C/N0 estimate */ + updated_common_data.cn0 = cn0_est(&data->cn0_est, cs[1].I/data->int_ms, cs[1].Q/data->int_ms); + if (updated_common_data.cn0 > track_cn0_drop_thres) + updated_common_data.cn0_above_drop_thres_count = updated_common_data.update_count; + + if (updated_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_channel_ambiguity_unknown(channel_info->context); + /* Update the latest time we were below the threshold. */ + updated_common_data.cn0_below_use_thres_count = updated_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) + updated_common_data.ld_opti_locked_count = updated_common_data.update_count; + if (!data->lock_detect.outp) + updated_common_data.ld_pess_unlocked_count = updated_common_data.update_count; + + /* Reset carrier phase ambiguity if there's doubt as to our phase lock */ + if (last_outp && !data->lock_detect.outp) + tracker_channel_ambiguity_unknown(channel_info->context); + + /* Run the loop filters. */ + + /* TODO: Make this more elegant. */ + 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; + } + + /* Output I/Q correlations using SBP if enabled for this channel */ + tracker_channel_correlations_send(channel_info->context, cs); + + aided_tl_update(&data->tl_state, cs2); + updated_common_data.carrier_freq = data->tl_state.carr_freq; + updated_common_data.code_phase_rate = data->tl_state.code_freq + GPS_CA_CHIPPING_RATE; + + data->code_phase_rate_fp_prev = data->code_phase_rate_fp; + data->code_phase_rate_fp = updated_common_data.code_phase_rate + * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; /* DT: shouldn't it be changed? */ + + data->carrier_freq_fp = updated_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 AND are in second-stage tracking) */ + 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("False phase lock detected on %s: err=%f", buf, err); + } + + tracker_channel_ambiguity_unknown(channel_info->context); + /* Indicate that a mode change has occurred. */ + updated_common_data.mode_change_count = updated_common_data.update_count; + + data->tl_state.carr_freq += err; + data->tl_state.carr_filt.y = data->tl_state.carr_freq; + } + } + + tracker_channel_retune(channel_info->context, data->carrier_freq_fp, + data->code_phase_rate_fp, + data->int_ms); //DT: to check this + + /* Update common data */ + tracker_common_data_update(channel_info->context, &updated_common_data); +} + +/** Parse a string describing the tracking loop filter parameters into + the loop_params_stage structs. */ +static bool parse_loop_params_l2cm(struct setting *s, const char *val) +{ + /** The string contains loop parameters for either one or two + stages. If the second is omitted, we'll use the same parameters + as the first 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."); + return false; + } + l->coherent_ms = tmp; + /* If string omits second-stage parameters, then after the first + stage has been parsed, n_chars_read == 0 because of missing + comma and we'll parse the string again into loop_params_parse[1]. */ + + if (l->coherent_ms != 20) { + log_error("Invalid coherent integration length for L2CM."); + return false; + } + /* Successfully parsed both stages. Save to memory. */ + strncpy(s->addr, val, s->len); + 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. +DT: at the moment this parser is same like for L1CA, +perhaps it's worth to reuse that one */ +static bool parse_lock_detect_params_l2cm(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."); + return false; + } + /* Successfully parsed. Save to memory. */ + strncpy(s->addr, val, s->len); + 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..4d728d14 --- /dev/null +++ b/src/track_gps_l2cm.h @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Jacob McNamee + * + * 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 From 43ed0d563dec6a457c903dfc22b008d6080702f8 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Mon, 21 Mar 2016 15:31:04 +0200 Subject: [PATCH 2/5] Intermediate commit --- src/main.c | 2 + src/manage.c | 10 ++-- src/simulator.c | 3 +- src/solution.c | 2 +- src/track.c | 4 +- src/track.h | 7 +++ src/track_gps_l2cm.c | 117 +++++++++++++++++++++---------------------- 7 files changed, 78 insertions(+), 67 deletions(-) diff --git a/src/main.c b/src/main.c index 5e80a5be..b2e81c11 100644 --- a/src/main.c +++ b/src/main.c @@ -47,6 +47,8 @@ extern void ext_setup(void); +void track_gps_l2cm_register(void) TRK_WEAK; + #if !defined(SYSTEM_CLOCK) #define SYSTEM_CLOCK 130944000 #endif diff --git a/src/manage.c b/src/manage.c index 527f62e3..b72ad3aa 100644 --- a/src/manage.c +++ b/src/manage.c @@ -263,8 +263,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 = -sid_to_carr_freq(sid) * + (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; @@ -695,8 +696,9 @@ static void manage_tracking_startup(void) /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ - track_count += 16 * (1023.0-cp) * - (1.0 + startup_params.carrier_freq / GPS_L1_HZ); + track_count += 16 * (1023.0-cp) * //DT: 1023 ?? + (1.0 + startup_params.carrier_freq / + sid_to_carr_freq(startup_params.sid)); /* Start the tracking channel */ if(!tracker_channel_init(chan, startup_params.sid, diff --git a/src/simulator.c b/src/simulator.c index 3ec9c66e..ea937e96 100644 --- a/src/simulator.c +++ b/src/simulator.c @@ -344,7 +344,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 / + sid_to_carr_freq(simulation_almanacs[almanac_i].sid)); 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 4d8ffe77..1fa6a3a8 100644 --- a/src/solution.c +++ b/src/solution.c @@ -524,7 +524,7 @@ static msg_t solution_thread(void *arg) /* Propagate observation to desired time. */ for (u8 i=0; iTOW_ms = TOW_INVALID; /* Calculate code phase rate with carrier aiding. */ - common_data->code_phase_rate = (1 + carrier_freq/GPS_L1_HZ) * + common_data->code_phase_rate = (1 + carrier_freq/GPS_L1_HZ) * //DT: to get rid of it GPS_CA_CHIPPING_RATE; common_data->carrier_freq = carrier_freq; diff --git a/src/track.h b/src/track.h index 73b2e07f..0bb3b17b 100644 --- a/src/track.h +++ b/src/track.h @@ -33,6 +33,13 @@ 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); diff --git a/src/track_gps_l2cm.c b/src/track_gps_l2cm.c index 9ae247ae..59c7900e 100644 --- a/src/track_gps_l2cm.c +++ b/src/track_gps_l2cm.c @@ -11,7 +11,7 @@ */ #include "track_gps_l2cm.h" -#include "track.h" +#include "track_api.h" #include #include @@ -28,7 +28,7 @@ carrier: nbw zeta k fll_aid */ #define GPS_L2CM_LOOP_PARAMS_MED \ - "(20 ms, (1, 0.7, 1, 1200), (13, 0.7, 1, 5))" + "(20 ms, (1, 0.7, 1, 1200), (13, 0.7, 1, 5))" /* k1, k2, lp, lo */ #define LD_PARAMS_PESS "0.10, 1.4, 200, 50" @@ -75,13 +75,13 @@ 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, - const tracker_common_data_t *common_data, + tracker_common_data_t *common_data, tracker_data_t *tracker_data); static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, - const tracker_common_data_t *common_data, + tracker_common_data_t *common_data, tracker_data_t *tracker_data); static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, - const tracker_common_data_t *common_data, + tracker_common_data_t *common_data, tracker_data_t *tracker_data); static bool parse_loop_params_l2cm(struct setting *s, const char *val); @@ -112,7 +112,6 @@ void track_gps_l2cm_register(void) SETTING("track", "cn0_drop", track_cn0_drop_thres, TYPE_FLOAT); SETTING("track", "alias_detect", use_alias_detection, TYPE_BOOL); - for (u32 i=0; icontext); + tracker_ambiguity_unknown(channel_info->context); const struct loop_params *l = &loop_params_stage; @@ -145,9 +142,8 @@ static void tracker_gps_l2cm_init(const tracker_channel_info_t *channel_info, l->carr_bw, l->carr_zeta, l->carr_k, l->carr_fll_aid_gain); - /*DT: I think NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ for L2C should be - * NAP_TRACK_NOMINAL_CODE_PHASE_RATE / 10.23e6, does HW team provide - * file (nap/track_channel.h) with these constants */ + /*DT: I think NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ for L2C should be + * differ than for L2C */ data->code_phase_rate_fp = common_data->code_phase_rate*NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; data->code_phase_rate_fp_prev = data->code_phase_rate_fp; data->carrier_freq_fp = (s32)(common_data->carrier_freq * NAP_TRACK_CARRIER_FREQ_UNITS_PER_HZ); @@ -162,7 +158,7 @@ static void tracker_gps_l2cm_init(const tracker_channel_info_t *channel_info, } static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, - const tracker_common_data_t *common_data, + tracker_common_data_t *common_data, tracker_data_t *tracker_data) { (void)channel_info; @@ -171,70 +167,65 @@ static void tracker_gps_l2cm_disable(const tracker_channel_info_t *channel_info, } static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, - const tracker_common_data_t *common_data, + tracker_common_data_t *common_data, tracker_data_t *tracker_data) { - gps_l2cm_tracker_data_t *data = tracker_data; char buf[SID_STR_LEN_MAX]; sid_to_string(buf, sizeof(buf), channel_info->sid); - tracker_channel_correlations_read(channel_info->context, data->cs, - &data->corr_sample_count); + tracker_correlations_read(channel_info->context, data->cs, + &data->corr_sample_count); alias_detect_first(&data->alias_detect, data->cs[1].I, data->cs[1].Q); - - tracker_common_data_t updated_common_data = *common_data; - - - updated_common_data.sample_count += data->corr_sample_count; - updated_common_data.code_phase_early = (u64)updated_common_data.code_phase_early + - (u64)data->corr_sample_count - * data->code_phase_rate_fp_prev; - updated_common_data.carrier_phase += (s64)data->carrier_freq_fp_prev - * data->corr_sample_count; + common_data->sample_count += data->corr_sample_count; + common_data->code_phase_early = (u64)common_data->code_phase_early + + (u64)data->corr_sample_count + * data->code_phase_rate_fp_prev; + common_data->carrier_phase += (s64)data->carrier_freq_fp_prev + * data->corr_sample_count; data->code_phase_rate_fp_prev = data->code_phase_rate_fp; data->carrier_freq_fp_prev = data->carrier_freq_fp; u8 int_ms = data->int_ms; - updated_common_data.TOW_ms = tracker_tow_update(channel_info->context, - updated_common_data.TOW_ms, - int_ms); - - updated_common_data.update_count += data->int_ms; + common_data->TOW_ms = tracker_tow_update(channel_info->context, + common_data->TOW_ms, + int_ms); - tracker_bit_sync_update(channel_info->context, int_ms, data->cs[1].I); + common_data->update_count += data->int_ms; + + tracker_bit_sync_update(channel_info->context, data->int_ms, data->cs[1].I); /* Correlations should already be in chan->cs thanks to * tracking_channel_get_corrs. */ corr_t* cs = data->cs; /* Update C/N0 estimate */ - updated_common_data.cn0 = cn0_est(&data->cn0_est, cs[1].I/data->int_ms, cs[1].Q/data->int_ms); - if (updated_common_data.cn0 > track_cn0_drop_thres) - updated_common_data.cn0_above_drop_thres_count = updated_common_data.update_count; + 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 (updated_common_data.cn0 < track_cn0_use_thres) { + 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_channel_ambiguity_unknown(channel_info->context); + tracker_ambiguity_unknown(channel_info->context); /* Update the latest time we were below the threshold. */ - updated_common_data.cn0_below_use_thres_count = updated_common_data.update_count; + 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) - updated_common_data.ld_opti_locked_count = updated_common_data.update_count; + common_data->ld_opti_locked_count = common_data->update_count; if (!data->lock_detect.outp) - updated_common_data.ld_pess_unlocked_count = updated_common_data.update_count; + 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) - tracker_channel_ambiguity_unknown(channel_info->context); + tracker_ambiguity_unknown(channel_info->context); /* Run the loop filters. */ @@ -246,21 +237,21 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, } /* Output I/Q correlations using SBP if enabled for this channel */ - tracker_channel_correlations_send(channel_info->context, cs); + tracker_correlations_send(channel_info->context, cs); aided_tl_update(&data->tl_state, cs2); - updated_common_data.carrier_freq = data->tl_state.carr_freq; - updated_common_data.code_phase_rate = data->tl_state.code_freq + GPS_CA_CHIPPING_RATE; + 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_prev = data->code_phase_rate_fp; - data->code_phase_rate_fp = updated_common_data.code_phase_rate + data->code_phase_rate_fp = common_data->code_phase_rate * NAP_TRACK_CODE_PHASE_RATE_UNITS_PER_HZ; /* DT: shouldn't it be changed? */ - data->carrier_freq_fp = updated_common_data.carrier_freq + 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 AND are in second-stage tracking) */ + /* 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); @@ -271,21 +262,29 @@ static void tracker_gps_l2cm_update(const tracker_channel_info_t *channel_info, log_warn("False phase lock detected on %s: err=%f", buf, err); } - tracker_channel_ambiguity_unknown(channel_info->context); + tracker_ambiguity_unknown(channel_info->context); /* Indicate that a mode change has occurred. */ - updated_common_data.mode_change_count = updated_common_data.update_count; + 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_channel_retune(channel_info->context, data->carrier_freq_fp, - data->code_phase_rate_fp, - data->int_ms); //DT: to check this + /* 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("%s synced @ %u ms, %.1f dBHz", + buf, (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; + } - /* Update common data */ - tracker_common_data_update(channel_info->context, &updated_common_data); + 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 @@ -327,8 +326,8 @@ static bool parse_loop_params_l2cm(struct setting *s, const char *val) } /** Parse a string describing the tracking loop phase lock detector - parameters into the lock_detect_params structs. -DT: at the moment this parser is same like for L1CA, + parameters into the lock_detect_params structs. +DT: at the moment this parser is same like for L1CA, perhaps it's worth to reuse that one */ static bool parse_lock_detect_params_l2cm(struct setting *s, const char *val) { From 9dd984c998e5720179738d6ff8ceed43ccb8d9f3 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Tue, 22 Mar 2016 15:56:02 +0200 Subject: [PATCH 3/5] Add initial version of L2C handover. Piksi v2 not broken --- libswiftnav | 2 +- src/manage.c | 9 ++--- src/simulator.c | 2 +- src/solution.c | 2 +- src/track.c | 5 +-- src/track.h | 3 +- src/track_gps_l1ca.c | 84 ++++++++++++++++++++++++++++++++++++++++++++ 7 files changed, 97 insertions(+), 10 deletions(-) diff --git a/libswiftnav b/libswiftnav index 59974796..4ea19a2a 160000 --- a/libswiftnav +++ b/libswiftnav @@ -1 +1 @@ -Subproject commit 59974796d6735efa1c842ccc373920cd9b78b447 +Subproject commit 4ea19a2af13138412e906ea37f2f3f96cd0ad77f diff --git a/src/manage.c b/src/manage.c index b72ad3aa..a166f5f2 100644 --- a/src/manage.c +++ b/src/manage.c @@ -263,7 +263,7 @@ 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 = -sid_to_carr_freq(sid) * + 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 */ @@ -692,13 +692,14 @@ static void manage_tracking_startup(void) float cp = propagate_code_phase(startup_params.code_phase, startup_params.carrier_freq, track_count - - startup_params.sample_count); + startup_params.sample_count, + startup_params.sid.code); /* Contrive for the timing strobe to occur at or close to a * PRN edge (code phase = 0) */ - track_count += 16 * (1023.0-cp) * //DT: 1023 ?? + track_count += 16 * (1023.0-cp) * (1.0 + startup_params.carrier_freq / - sid_to_carr_freq(startup_params.sid)); + code_to_carr_freq(startup_params.sid.code)); /* Start the tracking channel */ if(!tracker_channel_init(chan, startup_params.sid, diff --git a/src/simulator.c b/src/simulator.c index ea937e96..36f537ff 100644 --- a/src/simulator.c +++ b/src/simulator.c @@ -345,7 +345,7 @@ void populate_nav_meas(navigation_measurement_t *nav_meas, double dist, double e sim_settings.pseudorange_sigma); nav_meas->carrier_phase = dist / (GPS_C / - sid_to_carr_freq(simulation_almanacs[almanac_i].sid)); + 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 1fa6a3a8..710da950 100644 --- a/src/solution.c +++ b/src/solution.c @@ -524,7 +524,7 @@ static msg_t solution_thread(void *arg) /* Propagate observation to desired time. */ for (u8 i=0; i #include @@ -368,6 +371,87 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, /* Indicate that a mode change has occurred. */ common_data->mode_change_count = common_data->update_count; + + /* Transition to L2C tracking. */ + /* Since we have bitsync do handover to L2C if availble for the SV */ + /* First, get L2C capability for the SV from NDB + * TODO: since NDB not merged yet assume the the SV broadcasts L2C stream + * replace 0xffffffff by get_L2C_capability API */ + if (0xffffffff & (1 << channel_info->sid.sat)) { + /*find available tracking channel first*/ + s16 tk_ch = -1;//HACK for testing purposes -1; + + /* compose SID: same SV, but code is L2CA */ + gnss_signal_t sid = {.sat = channel_info->sid.sat, + .code = CODE_GPS_L2CM}; + for (u8 i=0; i -1) { + /* free tracking channel found */ + u32 track_count = nap_timing_count() + 20000; + + /* recalculate doppler freq for L2 from L1*/ + double carr_freq = tracking_channel_carrier_freq_get( + channel_info->nap_channel) * + GPS_L2_HZ / GPS_L1_HZ; + + log_debug("L2C Dopp %f, parent Dopp %f", + carr_freq, common_data->carrier_freq); + + /* recalculate code phase */ + float cp = propagate_code_phase(common_data->code_phase_early, + carr_freq, + track_count - + common_data->sample_count, + CODE_GPS_L2CM); + + log_debug("L2C CP %f, parent CP %f", + cp, common_data->code_phase_early); + + /* Contrive for the timing strobe to occur at or close to a + * PRN edge (code phase = 0) */ + track_count += 16 * (10230.0-cp) * + (1.0 + carr_freq / + code_to_carr_freq(CODE_GPS_L2CM)); + + /* get initial cn0 from parent L1 channel*/ + #if 1 + float cn0 = tracking_channel_cn0_get(channel_info->nap_channel); + + /* Start the tracking channel */ + if(!tracker_channel_init((tracker_channel_id_t)tk_ch, + sid, + carr_freq, + track_count, + cn0, + tracking_channel_evelation_degrees_get( + channel_info->nap_channel))) { + log_error("tracker channel init for L2C failed"); + } else + #endif + log_info("L2C handover done. Tracking channel %u, parent chnnel %u", + (u8)tk_ch, + channel_info->nap_channel); + + + /* TODO: Initialize elevation from ephemeris if we know it precisely */ + #if 1 + /* Start the decoder channel */ + if (!decoder_channel_init((u8)tk_ch, sid)) { + log_error("decoder channel init for L2C failed"); + } + #endif + } + else + log_warn("No free channel for L2C tracking"); + /* init tracking channel for L2C */ + } else + log_info("SV %u does not support L2C stream", channel_info->sid.sat); } tracker_retune(channel_info->context, data->carrier_freq_fp, From 213873b13f3a08a3fdc95d56b13648429389146d Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Wed, 23 Mar 2016 10:40:15 +0200 Subject: [PATCH 4/5] Intermediate commit --- src/track.c | 13 ++++++++----- src/track_gps_l1ca.c | 8 ++------ 2 files changed, 10 insertions(+), 11 deletions(-) diff --git a/src/track.c b/src/track.c index a770411e..5dc8c5f0 100644 --- a/src/track.c +++ b/src/track.c @@ -120,7 +120,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); @@ -233,7 +234,8 @@ float propagate_code_phase(float code_phase, float carrier_freq, * NOTE: the modulo is required to fix the fact rollover should * occur at 1023 not 1024. */ - return (float)((u32)(propagated_code_phase >> 28) % (1023*16)) / 16.0; + return (float)((u32)(propagated_code_phase >> 28) % + (code_to_chip_num(code)*16)) / 16.0; } /** Handles pending IRQs for the specified tracking channels. @@ -314,7 +316,7 @@ bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid, start_sample_count -= 0.5*16; common_data_init(&tracker_channel->common_data, start_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); @@ -969,7 +971,8 @@ static void event(tracker_channel_t *tracker_channel, event_t event) * \param cn0 C/N0 estimate. */ 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)); @@ -977,7 +980,7 @@ 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) * //DT: to get rid of it + common_data->code_phase_rate = (1 + carrier_freq/code_to_carr_freq(code)) * GPS_CA_CHIPPING_RATE; common_data->carrier_freq = carrier_freq; diff --git a/src/track_gps_l1ca.c b/src/track_gps_l1ca.c index 1d9f94a8..16b1c3e3 100644 --- a/src/track_gps_l1ca.c +++ b/src/track_gps_l1ca.c @@ -379,7 +379,7 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, * replace 0xffffffff by get_L2C_capability API */ if (0xffffffff & (1 << channel_info->sid.sat)) { /*find available tracking channel first*/ - s16 tk_ch = -1;//HACK for testing purposes -1; + s16 tk_ch = -1; /* compose SID: same SV, but code is L2CA */ gnss_signal_t sid = {.sat = channel_info->sid.sat, @@ -420,11 +420,10 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, code_to_carr_freq(CODE_GPS_L2CM)); /* get initial cn0 from parent L1 channel*/ - #if 1 float cn0 = tracking_channel_cn0_get(channel_info->nap_channel); /* Start the tracking channel */ - if(!tracker_channel_init((tracker_channel_id_t)tk_ch, + if (!tracker_channel_init((tracker_channel_id_t)tk_ch, sid, carr_freq, track_count, @@ -433,19 +432,16 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, channel_info->nap_channel))) { log_error("tracker channel init for L2C failed"); } else - #endif log_info("L2C handover done. Tracking channel %u, parent chnnel %u", (u8)tk_ch, channel_info->nap_channel); /* TODO: Initialize elevation from ephemeris if we know it precisely */ - #if 1 /* Start the decoder channel */ if (!decoder_channel_init((u8)tk_ch, sid)) { log_error("decoder channel init for L2C failed"); } - #endif } else log_warn("No free channel for L2C tracking"); From a8c055574d21ecf3d9af6e0103a5bfc48d71a110 Mon Sep 17 00:00:00 2001 From: Dmitry Tatarinov Date: Thu, 24 Mar 2016 14:13:00 +0200 Subject: [PATCH 5/5] Use L2C capability API --- src/track_gps_l1ca.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/track_gps_l1ca.c b/src/track_gps_l1ca.c index 16b1c3e3..7d4b5c0a 100644 --- a/src/track_gps_l1ca.c +++ b/src/track_gps_l1ca.c @@ -374,10 +374,10 @@ static void tracker_gps_l1ca_update(const tracker_channel_info_t *channel_info, /* Transition to L2C tracking. */ /* Since we have bitsync do handover to L2C if availble for the SV */ - /* First, get L2C capability for the SV from NDB - * TODO: since NDB not merged yet assume the the SV broadcasts L2C stream - * replace 0xffffffff by get_L2C_capability API */ - if (0xffffffff & (1 << channel_info->sid.sat)) { + /* First, get L2C capability for the SV from NDB */ + u32 l2c_cpbl; + ndb_gps_l2cm_l2c_cap_read(&l2c_cpbl); + if (l2c_cpbl & (1 << channel_info->sid.sat)) { /*find available tracking channel first*/ s16 tk_ch = -1;