From f6a1b299e4ebe9a59a803ff5b56d88748a0c97b5 Mon Sep 17 00:00:00 2001 From: dzollo Date: Tue, 24 May 2016 20:35:42 -0700 Subject: [PATCH 1/3] calculate lock_counter --- include/libswiftnav/track.h | 5 +++-- src/track.c | 17 +++++++++++++++++ 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index f41cb3a7..c429ba30 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -156,6 +156,7 @@ typedef struct { ambiguity is reset. If this number changes it is an indication you should reset integer ambiguity resolution for this channel. */ + u32 lock_time; /**< Milliseconds that channel has been locked. **/ } channel_measurement_t; typedef struct { @@ -168,7 +169,7 @@ typedef struct { double sat_pos[3]; double sat_vel[3]; double snr; - double lock_time; + u32 lock_time; gps_time_t tot; gnss_signal_t sid; u16 lock_counter; @@ -242,5 +243,5 @@ int nav_meas_cmp(const void *a, const void *b); u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new, u8 n_old, navigation_measurement_t *m_old, navigation_measurement_t *m_corrected, double dt); - +bool calculate_loss_of_lock(u32 dt, u32 prev_lock_time, u32 curr_lock_time); #endif /* LIBSWIFTNAV_TRACK_H */ diff --git a/src/track.c b/src/track.c index e28db0c7..75ec6a53 100644 --- a/src/track.c +++ b/src/track.c @@ -860,6 +860,18 @@ int nav_meas_cmp(const void *a, const void *b) ((navigation_measurement_t*)b)->sid); } +bool calculate_loss_of_lock(u32 dt, u32 prev_lock_time, u32 curr_lock_time) { + if (prev_lock_time > curr_lock_time) return true; + else if ((prev_lock_time == curr_lock_time) && (dt >= prev_lock_time)) return true; + else if ((prev_lock_time == curr_lock_time) && (dt < prev_lock_time)) return false; + else if ((prev_lock_time < curr_lock_time) && \ + (dt >= (2 * curr_lock_time - prev_lock_time))) return true; + else if ((prev_lock_time < curr_lock_time) && \ + (curr_lock_time < dt && dt < (2 * curr_lock_time - prev_lock_time))) return true; + else if ((prev_lock_time < curr_lock_time) && (dt <= curr_lock_time)) return false; + else return true; +} + /** Set measurement precise Doppler using time difference of carrier phase. * \note The return array `m_tdcp` should have space to contain the number * of measurements with common PRNs between `m_new` and `m_old`. Making the @@ -899,6 +911,11 @@ u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new, /* Re-apply the same correction to the raw Doppler to get the corrected Doppler. */ m_corrected[n].doppler = (m_new[i].carrier_phase - m_old[j].carrier_phase) / dt + dopp_corr; + + /* set the lock_counter according to whether a slip could have occured */ + if (calculate_loss_of_lock(dt, m_new[i].lock_time, m_old[j].lock_time)) { + m_corrected[n].lock_counter +=1; + } n++; } } From c3833fc21b2306f2581ebea8a2c100f93fa1eae8 Mon Sep 17 00:00:00 2001 From: dzollo Date: Wed, 25 May 2016 17:17:16 -0700 Subject: [PATCH 2/3] new obs: lock counter --- src/track.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/track.c b/src/track.c index 75ec6a53..0b4f648a 100644 --- a/src/track.c +++ b/src/track.c @@ -804,7 +804,7 @@ s8 calc_navigation_measurement(u8 n_channels, const channel_measurement_t *meas[ /* Copy over remaining values. */ nav_meas[i]->snr = meas[i]->snr; nav_meas[i]->lock_counter = meas[i]->lock_counter; - + nav_meas[i]->lock_time = meas[i]->lock_time; /* calc sat clock error */ if (calc_sat_state(e[i], &nav_meas[i]->tot, nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, @@ -860,7 +860,7 @@ int nav_meas_cmp(const void *a, const void *b) ((navigation_measurement_t*)b)->sid); } -bool calculate_loss_of_lock(u32 dt, u32 prev_lock_time, u32 curr_lock_time) { +bool calculate_loss_of_lock(double dt, u32 prev_lock_time, u32 curr_lock_time) { if (prev_lock_time > curr_lock_time) return true; else if ((prev_lock_time == curr_lock_time) && (dt >= prev_lock_time)) return true; else if ((prev_lock_time == curr_lock_time) && (dt < prev_lock_time)) return false; @@ -911,10 +911,13 @@ u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new, /* Re-apply the same correction to the raw Doppler to get the corrected Doppler. */ m_corrected[n].doppler = (m_new[i].carrier_phase - m_old[j].carrier_phase) / dt + dopp_corr; - + m_corrected[n].lock_time = m_new[i].lock_time; + m_corrected[n].lock_counter = m_old[j].lock_counter; /* set the lock_counter according to whether a slip could have occured */ - if (calculate_loss_of_lock(dt, m_new[i].lock_time, m_old[j].lock_time)) { - m_corrected[n].lock_counter +=1; + if (calculate_loss_of_lock(dt*1000.0, m_old[j].lock_time, m_new[i].lock_time)) { + log_info("tdcp %u:%u, dt %f", m_new[i].lock_time, m_old[j].lock_time, dt*1000.0); + m_corrected[n].lock_counter = m_new[i].lock_counter + 1; + m_new[i].lock_counter += 1; } n++; } From 2ab532a918a4ee5aa0c597140dee4ae283f3583c Mon Sep 17 00:00:00 2001 From: dzollo Date: Wed, 25 May 2016 17:17:44 -0700 Subject: [PATCH 3/3] new obs: function prototype --- include/libswiftnav/track.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index c429ba30..fd4b9dfe 100644 --- a/include/libswiftnav/track.h +++ b/include/libswiftnav/track.h @@ -243,5 +243,5 @@ int nav_meas_cmp(const void *a, const void *b); u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new, u8 n_old, navigation_measurement_t *m_old, navigation_measurement_t *m_corrected, double dt); -bool calculate_loss_of_lock(u32 dt, u32 prev_lock_time, u32 curr_lock_time); +bool calculate_loss_of_lock(double dt, u32 prev_lock_time, u32 curr_lock_time); #endif /* LIBSWIFTNAV_TRACK_H */