Merge pull request #10 from swift-nav/pr-699-copy

Change solution to count satellites, not tracking channels. Verified on VV-1606301020.

Author: valeri-atamaniouk

src/Makefile

@@ -123,6 +123,7 @@ CSRC := $(STARTUPSRC) \
         $(SWIFTNAV_ROOT)/src/decode.o \
         $(SWIFTNAV_ROOT)/src/signal.o \
         $(SWIFTNAV_ROOT)/src/l2c_capb.o \
+        $(SWIFTNAV_ROOT)/src/sid_set.o \
         main.c
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global

src/sid_set.c

@@ -0,0 +1,63 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Roman Gezikov <rgezikov@exafore.com>
+ *
+ * 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 <assert.h>
+#include <string.h>
+#include <libswiftnav/bits.h>
+#include "sid_set.h"
+
+/** Initialize new sid set
+ *
+ * \param sid_list gnss_sid_set_t to be initialized
+ *
+ */
+void sid_set_init(gnss_sid_set_t *sid_set)
+{
+  memset(sid_set->sats, 0, sizeof(sid_set->sats));
+}
+
+/** Add new element to the set
+ *
+ * \param sid_set gnss_sid_set_t to add new element to
+ * \param sid element to be added
+ *
+ */
+void sid_set_add(gnss_sid_set_t *sid_set, gnss_signal_t sid)
+{
+  u16 s = sid_to_code_index(sid);
+  assert(s < 64);
+  sid_set->sats[sid.code] |= ((u64)0x01 << s);
+}
+
+/** Get number of satellites in sid set
+ *
+ * \param sid_set gnss_sid_set_t to count number of satellites in
+ *
+ * \returns Number of unique satellites present in the set.
+ *
+ */
+u32 sid_set_get_sat_count(const gnss_sid_set_t *sid_set)
+{
+  u64 sats[CONSTELLATION_COUNT];
+  memset(sats, 0, sizeof(sats));
+  for (enum code code = 0; code < CODE_COUNT; code++) {
+    sats[code_to_constellation(code)] |= sid_set->sats[code];
+  }
+
+  u32 cnt = 0;
+  for (enum constellation constellation = 0;
+       constellation < CONSTELLATION_COUNT; constellation++) {
+    cnt += count_bits_u64(sats[constellation], 1);
+  }
+
+  return cnt;
+}

src/sid_set.h

@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Roman Gezikov <rgezikov@exafore.com>
+ *
+ * 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_SID_SET_H
+#define SWIFTNAV_SID_SET_H
+
+#include <libswiftnav/common.h>
+#include <libswiftnav/signal.h>
+
+typedef struct {
+  u64 sats[CODE_COUNT];
+} gnss_sid_set_t;
+
+void sid_set_init(gnss_sid_set_t *sid_set);
+void sid_set_add(gnss_sid_set_t *sid_set, gnss_signal_t sid);
+u32 sid_set_get_sat_count(const gnss_sid_set_t *sid_set);
+
+#endif /* SWIFTNAV_SID_SET_H */

src/solution.c

@@ -43,6 +43,7 @@
 #include "signal.h"
 #include "system_monitor.h"
 #include "main.h"
+#include "sid_set.h"
 
 /* Maximum CPU time the solution thread is allowed to use. */
 #define SOLN_THD_CPU_MAX (0.60f)
@@ -462,7 +463,12 @@ static void solution_thread(void *arg)
       tracking_channel_unlock(i);
     }
 
-    if (n_ready < 4) {
+    gnss_sid_set_t codes_in_track;
+    sid_set_init(&codes_in_track);
+    for (u8 i=0; i<n_ready; i++)
+      sid_set_add(&codes_in_track, meas[i].sid);
+
+    if (sid_set_get_sat_count(&codes_in_track) < 4) {
       /* Not enough sats, keep on looping. */
       continue;
     }
@@ -517,7 +523,13 @@ static void solution_thread(void *arg)
     n_ready_old = n_ready;
     rec_tc_old = rec_tc;
 
-    if (n_ready_tdcp < 4) {
+    gnss_sid_set_t codes_tdcp;
+    sid_set_init(&codes_tdcp);
+    for (u8 i=0; i<n_ready_tdcp; i++) {
+      sid_set_add(&codes_tdcp, nav_meas_tdcp[i].sid);
+    }
+
+    if (sid_set_get_sat_count(&codes_tdcp) < 4) {
       /* Not enough sats to compute PVT */
       continue;
     }