Add Nav DB module

src/Makefile

@@ -134,6 +134,8 @@ CSRC := $(PORTSRC) \
         $(SWIFTNAV_ROOT)/src/decode.o \
         $(SWIFTNAV_ROOT)/src/decode_gps_l1.o \
         $(SWIFTNAV_ROOT)/src/signal.o \
+        $(SWIFTNAV_ROOT)/src/ndb.o \
+        $(SWIFTNAV_ROOT)/src/ndb_p1.o \
         main.c
 
 # C++ sources that can be compiled in ARM or THUMB mode depending on the global

src/base_obs.c

@@ -35,6 +35,7 @@
 #include "base_obs.h"
 #include "ephemeris.h"
 #include "signal.h"
+#include "ndb.h"
 
 extern bool disable_raim;
 
@@ -300,9 +301,9 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context)
 
     /* Check if we have an ephemeris for this satellite, we will need this to
      * fill in satellite position etc. parameters. */
-    ephemeris_lock();
-    ephemeris_t *e = ephemeris_get(sid);
-    if (ephemeris_valid(e, &t)) {
+    ephemeris_t ephe;
+    if((ndb_ephemeris_read(sid, &ephe) == NDB_ERR_NONE) &&
+       (ephemeris_valid(&ephe, &t))) {
       /* Unpack the observation into a navigation_measurement_t. */
       unpack_obs_content(
         &obs[i],
@@ -315,7 +316,7 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context)
       double clock_err;
       double clock_rate_err;
       /* Calculate satellite parameters using the ephemeris. */
-      calc_sat_state(e, &t,
+      calc_sat_state(&ephe, &t,
                      base_obss_rx.nm[base_obss_rx.n].sat_pos,
                      base_obss_rx.nm[base_obss_rx.n].sat_vel,
                      &clock_err, &clock_rate_err);
@@ -328,7 +329,6 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context)
       base_obss_rx.nm[base_obss_rx.n].tot = t;
       base_obss_rx.n++;
     }
-    ephemeris_unlock();
   }
 
   /* If we can, and all the obs have been received, update to using the new

src/decode_gps_l1.c

@@ -18,6 +18,7 @@
 #include "sbp.h"
 #include "sbp_utils.h"
 #include "decode.h"
+#include "ndb.h"
 
 #define NUM_GPS_L1_DECODERS   12
 
@@ -112,10 +113,11 @@ static void decoder_gps_l1_process(const decoder_channel_info_t *channel_info,
   /* Decoded a new ephemeris. */
   ephemeris_new(&e);
 
-  ephemeris_t *eph = ephemeris_get(channel_info->sid);
-  if (!eph->valid) {
+  u8 v, h;
+  ndb_ephemeris_info(channel_info->sid, &v, &h, NULL, NULL);
+  if (!v) {
     char buf[SID_STR_LEN_MAX];
     sid_to_string(buf, sizeof(buf), channel_info->sid);
-    log_info("%s ephemeris is invalid", buf);
+    log_info("%s ephemeris wasn't stored", buf);
   }
 }

src/ephemeris.c

@@ -10,7 +10,6 @@
  * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
  * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
  */
-
 #include <string.h>
 #include <libswiftnav/ephemeris.h>
 #include <libswiftnav/logging.h>
@@ -23,56 +22,9 @@
 #include "timing.h"
 #include "ephemeris.h"
 #include "signal.h"
+#include "ndb.h"
 
-#define EPHEMERIS_TRANSMIT_EPOCH_SPACING_ms   (15 * 1000)
-#define EPHEMERIS_MESSAGE_SPACING_ms          (200)
-
-MUTEX_DECL(es_mutex);
-static ephemeris_t es[PLATFORM_SIGNAL_COUNT] _CCM;
-static ephemeris_t es_candidate[PLATFORM_SIGNAL_COUNT] _CCM;
-
-static WORKING_AREA_CCM(wa_ephemeris_thread, 1400);
-
-static msg_t ephemeris_thread(void *arg);
-
-static msg_t ephemeris_thread(void *arg)
-{
-  (void)arg;
-  chRegSetThreadName("ephemeris");
-
-  systime_t tx_epoch = chTimeNow();
-  while (1) {
-
-    for (u32 i=0; i<PLATFORM_SIGNAL_COUNT; i++) {
-      bool success = false;
-      const ephemeris_t *e = &es[i];
-      gps_time_t t = get_current_time();
-
-      /* Quickly check validity before locking */
-      if (ephemeris_valid(e, &t)) {
-        ephemeris_lock();
-        /* Now that we are locked, reverify validity and transmit */
-        if (ephemeris_valid(e, &t)) {
-          msg_ephemeris_t msg;
-          pack_ephemeris(e, &msg);
-          sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *)&msg);
-          success = true;
-        }
-        ephemeris_unlock();
-      }
-
-      if (success) {
-        chThdSleep(MS2ST(EPHEMERIS_MESSAGE_SPACING_ms));
-      }
-    }
-
-    // wait for the next transmit epoch
-    tx_epoch += MS2ST(EPHEMERIS_TRANSMIT_EPOCH_SPACING_ms);
-    chThdSleepUntil(tx_epoch);
-  }
-
-  return 0;
-}
+static ephemeris_t es_candidate[PLATFORM_SIGNAL_COUNT];
 
 void ephemeris_new(ephemeris_t *e)
 {
@@ -81,28 +33,23 @@ void ephemeris_new(ephemeris_t *e)
   char buf[SID_STR_LEN_MAX];
   sid_to_string(buf, sizeof(buf), e->sid);
 
-  gps_time_t t = get_current_time();
+  if (!e->valid) {
+    log_error("Invalid ephemeris for %s", buf);
+    return;
+  }
+
   u32 index = sid_to_global_index(e->sid);
-  if (!ephemeris_valid(&es[index], &t)) {
-    /* Our currently used ephemeris is bad, so we assume this is better. */
-    log_info("New untrusted ephemeris for %s", buf);
-    ephemeris_lock();
-    es[index] = es_candidate[index] = *e;
-    ephemeris_unlock();
 
-  } else if (ephemeris_equal(&es_candidate[index], e)) {
+  if(ephemeris_equal(&es_candidate[index], e)) {
     /* The received ephemeris matches our candidate, so we trust it. */
     log_info("New trusted ephemeris for %s", buf);
-    ephemeris_lock();
-    es[index] = *e;
-    ephemeris_unlock();
+    if (ndb_ephemeris_store(e, NDB_DS_RECEIVER) != NDB_ERR_NONE)
+      log_error("Error storing ephemeris for %s", buf);
   } else {
     /* This is our first reception of this new ephemeris, so treat it with
      * suspicion and call it the new candidate. */
     log_info("New ephemeris candidate for %s", buf);
-    ephemeris_lock();
     es_candidate[index] = *e;
-    ephemeris_unlock();
   }
 }
 
@@ -121,42 +68,18 @@ static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* contex
     log_warn("Ignoring ephemeris for invalid sat");
     return;
   }
-
-  ephemeris_new(&e);
+  /* We trust epehemeris that we received over SBP, so save it to NDB right
+   * away. If we receive new one from the sky twice it will replace it. */
+  ndb_ephemeris_store(&e, NDB_DS_SBP);
 }
 
 void ephemeris_setup(void)
 {
   memset(es_candidate, 0, sizeof(es_candidate));
-  memset(es, 0, sizeof(es));
-  for (u32 i=0; i<PLATFORM_SIGNAL_COUNT; i++) {
-    es[i].sid = sid_from_global_index(i);
-  }
-
   static sbp_msg_callbacks_node_t ephemeris_msg_node;
   sbp_register_cbk(
     SBP_MSG_EPHEMERIS,
     &ephemeris_msg_callback,
     &ephemeris_msg_node
   );
-
-  chThdCreateStatic(wa_ephemeris_thread, sizeof(wa_ephemeris_thread),
-                    NORMALPRIO-10, ephemeris_thread, NULL);
-}
-
-void ephemeris_lock(void)
-{
-  chMtxLock(&es_mutex);
-}
-
-void ephemeris_unlock(void)
-{
-  Mutex *m = chMtxUnlock();
-  assert(m == &es_mutex);
-}
-
-ephemeris_t *ephemeris_get(gnss_signal_t sid)
-{
-  assert(sid_supported(sid));
-  return &es[sid_to_global_index(sid)];
 }

src/ephemeris.h

@@ -18,10 +18,6 @@
 #include <libswiftnav/signal.h>
 
 void ephemeris_setup(void);
-void ephemeris_lock(void);
-void ephemeris_unlock(void);
 void ephemeris_new(ephemeris_t *e);
-ephemeris_t *ephemeris_get(gnss_signal_t sid);
-
 #endif
 

src/main.c

@@ -43,6 +43,7 @@
 #include "decode.h"
 #include "decode_gps_l1.h"
 #include "signal.h"
+#include "ndb.h"
 
 extern void ext_setup(void);
 
@@ -223,6 +224,7 @@ int main(void)
   READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT);
 
   ephemeris_setup();
+  ndb_init();
 
   /* Send message to inform host we are up and running. */
   u32 startup_flags = 0;

src/manage.c

@@ -43,6 +43,7 @@
 #include "./system_monitor.h"
 #include "settings.h"
 #include "signal.h"
+#include "ndb.h"
 
 /** \defgroup manage Manage
  * Manage acquisition and tracking.
@@ -112,9 +113,6 @@ typedef struct {
 static tracking_startup_fifo_t tracking_startup_fifo;
 
 static MUTEX_DECL(tracking_startup_mutex);
-
-static almanac_t almanac[PLATFORM_SIGNAL_COUNT];
-
 static float elevation_mask = 0.0; /* degrees */
 static bool sbas_enabled = false;
 
@@ -194,8 +192,6 @@ void manage_acq_setup()
     acq_status[i].sid = sid_from_global_index(i);
 
     track_mask[i] = false;
-    almanac[i].valid = 0;
-
     if (!sbas_enabled &&
         (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) {
       acq_status[i].masked = true;
@@ -250,10 +246,11 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t,
 
     /* Do we have a suitable ephemeris for this sat?  If so, use
        that in preference to the almanac. */
-    const ephemeris_t *e = ephemeris_get(sid);
-    if (ephemeris_valid(e, t)) {
+    ephemeris_t ephe;
+    enum ndb_op_code oc = ndb_ephemeris_read(sid, &ephe);
+    if((NDB_ERR_NONE == oc) && ephemeris_valid(&ephe, t)) {
       double sat_pos[3], sat_vel[3], el_d;
-      calc_sat_state(e, t, sat_pos, sat_vel, &_, &_);
+      calc_sat_state(&ephe, t, sat_pos, sat_vel, &_, &_);
       wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d);
       el = (float)(el_d) * R2D;
       if (el < elevation_mask)
@@ -269,14 +266,15 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t,
       if (time_quality >= TIME_FINE)
         dopp_uncertainty = DOPP_UNCERT_EPHEM;
     } else {
-      const almanac_t *a = &almanac[sid_to_global_index(sid)];
-      if (a->valid) {
-        calc_sat_az_el_almanac(a, t->tow, t->wn-1024,
+      almanac_t alma;
+      oc = ndb_almanac_read(sid, &alma);
+      if((NDB_ERR_NONE == oc) && (alma.valid)) {
+        calc_sat_az_el_almanac(&alma, t->tow, t->wn-1024,
                                position_solution.pos_ecef, &_, &el_d);
         el = (float)(el_d) * R2D;
         if (el < elevation_mask)
           return SCORE_BELOWMASK;
-        dopp_hint = -calc_sat_doppler_almanac(a, t->tow, t->wn,
+        dopp_hint = -calc_sat_doppler_almanac(&alma, t->tow, t->wn,
                                               position_solution.pos_ecef);
       } else {
         return SCORE_COLDSTART; /* Couldn't determine satellite state. */
@@ -532,9 +530,9 @@ static void manage_track()
     }
 
     /* Is ephemeris or alert flag marked unhealthy?*/
-    const ephemeris_t *e = ephemeris_get(sid);
-    /* TODO: check alert flag */
-    if (e->valid && !satellite_healthy(e)) {
+    u8 v, h;
+    enum ndb_op_code oc = ndb_ephemeris_info(sid, &v, &h, NULL, NULL);
+    if((NDB_ERR_NONE == oc) && v && !h) {
       log_info("%s unhealthy, dropping", buf);
       drop_channel(i);
       acq->state = ACQ_PRN_UNHEALTHY;
@@ -608,9 +606,9 @@ s8 use_tracking_channel(u8 i)
       /* Channel time of week has been decoded. */
       && (tracking_channel_tow_ms_get(i) != TOW_INVALID)
       /* Nav bit polarity is known, i.e. half-cycles have been resolved. */
-      && tracking_channel_bit_polarity_resolved(i))
+      && tracking_channel_bit_polarity_resolved(i)) {
       /* TODO: Alert flag is not set */
-      {
+
     /* Ephemeris must be valid, not stale. Satellite must be healthy.
        This also acts as a sanity check on the channel TOW.*/
     gps_time_t t = {
@@ -619,9 +617,14 @@ s8 use_tracking_channel(u8 i)
       .wn = WN_UNKNOWN,
       .tow = 1e-3 * tracking_channel_tow_ms_get(i)
     };
-    ephemeris_t *e = ephemeris_get(tracking_channel_sid_get(i));
-    return ephemeris_valid(e, &t) && satellite_healthy(e);
-  } else return 0;
+    gnss_signal_t sid = tracking_channel_sid_get(i);
+    u8 v, h, fit_interval;
+    gps_time_t toe;
+    ndb_ephemeris_info(sid, &v, &h, &toe, &fit_interval);
+    return ephemeris_params_valid(v, fit_interval, &toe, &t) && h;
+  }
+
+  return 0;
 }
 
 u8 tracking_channels_ready()