diff --git a/include/libswiftnav/track.h b/include/libswiftnav/track.h index f41cb3a7..fd4b9dfe 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(double 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..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,6 +860,18 @@ int nav_meas_cmp(const void *a, const void *b) ((navigation_measurement_t*)b)->sid); } +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; + 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,14 @@ 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*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++; } }