New track field

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 */

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++;
     }
   }