Merge pull request #572 from jacobmcnamee/generalize

Generalize data structures PRN -> SID, SBAS support

Author: jacobmcnamee

libswiftnav

@@ -29,18 +29,18 @@
 
 static BinarySemaphore load_wait_sem;
 
-void acq_set_prn(u8 prn)
+void acq_set_sid(gnss_signal_t sid)
 {
   chBSemInit(&load_wait_sem, TRUE);
-  nap_acq_code_wr_blocking(prn);
+  nap_acq_code_wr_blocking(sid);
   if (chBSemWaitTimeout(&load_wait_sem, 1000) == RDY_TIMEOUT) {
     log_error("acq: Timeout waiting for code load!");
   }
 }
 
 /** Send results of an acquisition to the host.
  *
- * \param prn  PRN (0-31) of the acquisition
+ * \param sid SID of the acquisition
  * \param snr Signal to noise ratio of best point from acquisition.
  * \param cp  Code phase of best point.
  * \param cf  Carrier frequency of best point.

src/acq.c

@@ -16,7 +16,7 @@
 #include <libswiftnav/common.h>
 #include <libswiftnav/signal.h>
 
-void acq_set_prn(u8 prn);
+void acq_set_sid(gnss_signal_t sid);
 
 bool acq_load(u32 count);
 void acq_service_load_done(void);

src/acq.h

@@ -20,6 +20,7 @@
 #include <libswiftnav/ephemeris.h>
 #include <libswiftnav/coord_system.h>
 #include <libswiftnav/linear_algebra.h>
+#include <libswiftnav/signal.h>
 
 #include "board/leds.h"
 #include "position.h"
@@ -289,19 +290,18 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context)
   /* Pull out the contents of the message. */
   packed_obs_content_t *obs = (packed_obs_content_t *)(msg + sizeof(observation_header_t));
   for (u8 i=0; i<obs_in_msg; i++) {
-    /* Check the PRN is valid. e.g. simulation mode outputs test observations
-     * with PRNs >200. */
-    if (obs[i].sid.sat > 31) { /* TODO prn - sid; assume everything below is 0x1F masked! */
+    gnss_signal_t sid = sid_from_sbp(obs[i].sid);
+    if (!sid_valid(sid))
       continue;
-    }
 
     /* Flag this as visible/viable to acquisition/search */
-    manage_set_obs_hint(sid_from_sbp(obs[i].sid));
+    manage_set_obs_hint(sid);
 
     /* Check if we have an ephemeris for this satellite, we will need this to
      * fill in satellite position etc. parameters. */
-    chMtxLock(&es_mutex);
-    if (ephemeris_good(&es[obs[i].sid.sat], t)) {
+    ephemeris_lock();
+    ephemeris_t *e = ephemeris_get(sid);
+    if (ephemeris_good(e, t)) {
       /* Unpack the observation into a navigation_measurement_t. */
       unpack_obs_content(
         &obs[i],
@@ -314,7 +314,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(&es[obs[i].sid.sat], t,
+      calc_sat_state(e, 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);
@@ -327,7 +327,7 @@ 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++;
     }
-    chMtxUnlock();
+    ephemeris_unlock();
   }
 
   /* If we can, and all the obs have been received, update to using the new

src/base_obs.c

@@ -127,11 +127,11 @@ void nap_acq_corr_rd_blocking(u16 *index, u16 *max, float *ave)
  * CA Code for SV to be searched for must be written into channel's code ram
  * before acquisitions are started.
  *
- * \param prn PRN number (0-31) of CA code to be written.
+ * \param sid Signal identifier corresponding to CA code to be written.
  */
-void nap_acq_code_wr_blocking(u8 prn)
+void nap_acq_code_wr_blocking(gnss_signal_t sid)
 {
-  nap_xfer_blocking(NAP_REG_ACQ_CODE, 128, 0, ca_code(prn));
+  nap_xfer_blocking(NAP_REG_ACQ_CODE, 128, 0, ca_code(sid));
 }
 
 /** \} */

src/board/nap/acq_channel.c

@@ -15,6 +15,7 @@
 #define SWIFTNAV_ACQ_CHANNEL_H
 
 #include <libswiftnav/common.h>
+#include <libswiftnav/signal.h>
 
 #include "../../main.h"
 #include "nap_common.h"
@@ -51,7 +52,7 @@ void nap_acq_load_wr_disable_blocking(void);
 void nap_acq_init_wr_params_blocking(s16 carrier_freq);
 void nap_acq_init_wr_disable_blocking(void);
 void nap_acq_corr_rd_blocking(u16 *index, u16 *max, float *ave);
-void nap_acq_code_wr_blocking(u8 prn);
+void nap_acq_code_wr_blocking(gnss_signal_t sid);
 
 #endif  /* SWIFTNAV_ACQ_CHANNEL_H */
 

src/board/nap/acq_channel.h

@@ -221,10 +221,10 @@ void nap_track_phase_rd_blocking(u8 channel, s32* carrier_phase,
  *
  * \param prn PRN number (0-31) of CA code to be written.
  */
-void nap_track_code_wr_blocking(u8 channel, u8 prn)
+void nap_track_code_wr_blocking(u8 channel, gnss_signal_t sid)
 {
   nap_xfer_blocking(NAP_REG_TRACK_BASE + channel * NAP_TRACK_N_REGS
-                     + NAP_REG_TRACK_CODE_OFFSET, 128, 0, ca_code(prn));
+                     + NAP_REG_TRACK_CODE_OFFSET, 128, 0, ca_code(sid));
 }
 
 /** \} */

src/board/nap/track_channel.c

@@ -15,6 +15,7 @@
 #define SWIFTNAV_TRACK_CHANNEL_H
 
 #include <libswiftnav/common.h>
+#include <libswiftnav/signal.h>
 
 #include "../../main.h"
 #include "nap_common.h"
@@ -76,7 +77,7 @@ void nap_track_corr_rd_blocking(u8 channel, u32* sample_count, corr_t corrs[]);
 void nap_track_phase_unpack(u8 packed[], s32* carrier_phase, u64* code_phase);
 void nap_track_phase_rd_blocking(u8 channel, s32* carrier_phase,
                                  u64* code_phase);
-void nap_track_code_wr_blocking(u8 channel, u8 prn);
+void nap_track_code_wr_blocking(u8 channel, gnss_signal_t sid);
 
 #endif  /* SWIFTNAV_TRACK_CHANNEL_H */
 

src/board/nap/track_channel.h

@@ -15,6 +15,7 @@
 #include <libswiftnav/ephemeris.h>
 #include <libswiftnav/logging.h>
 #include <ch.h>
+#include <assert.h>
 
 #include "sbp.h"
 #include "sbp_utils.h"
@@ -23,32 +24,35 @@
 #include "ephemeris.h"
 
 MUTEX_DECL(es_mutex);
-ephemeris_t es[MAX_SATS] _CCM;
-static ephemeris_t es_candidate[MAX_SATS] _CCM;
+static ephemeris_t es[NUM_SATS] _CCM;
+static ephemeris_t es_candidate[NUM_SATS] _CCM;
 
 static void ephemeris_new(ephemeris_t *e)
 {
+  assert(sid_valid(e->sid));
+
   gps_time_t t = get_current_time();
-  if (!ephemeris_good(&es[e->sid.sat], t)) {
+  u32 index = sid_to_index(e->sid);
+  if (!ephemeris_good(&es[index], t)) {
     /* Our currently used ephemeris is bad, so we assume this is better. */
     log_info("New untrusted ephemeris for PRN %02d", e->sid.sat+1);
-    chMtxLock(&es_mutex);
-    es[e->sid.sat] = es_candidate[e->sid.sat] = *e;
-    chMtxUnlock();
+    ephemeris_lock();
+    es[index] = es_candidate[index] = *e;
+    ephemeris_unlock();
 
-  } else if (ephemeris_equal(&es_candidate[e->sid.sat], e)) {
+  } else if (ephemeris_equal(&es_candidate[index], e)) {
     /* The received ephemeris matches our candidate, so we trust it. */
     log_info("New trusted ephemeris for PRN %02d", e->sid.sat+1);
-    chMtxLock(&es_mutex);
-    es[e->sid.sat] = *e;
-    chMtxUnlock();
+    ephemeris_lock();
+    es[index] = *e;
+    ephemeris_unlock();
   } 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 PRN %02d", e->sid.sat+1);
-    chMtxLock(&es_mutex);
-    es_candidate[e->sid.sat] = *e;
-    chMtxUnlock();
+    ephemeris_lock();
+    es_candidate[index] = *e;
+    ephemeris_unlock();
   }
 }
 
@@ -86,11 +90,12 @@ static msg_t nav_msg_thread(void *arg)
       /* Decoded a new ephemeris. */
       ephemeris_new(&e);
 
-      if (!es[ch->sid.sat].healthy) {
+      ephemeris_t *eph = ephemeris_get(ch->sid);
+      if (!eph->healthy) {
         log_info("PRN %02d unhealthy", ch->sid.sat+1);
       } else {
         msg_ephemeris_t msg;
-        pack_ephemeris(&es[ch->sid.sat], &msg);
+        pack_ephemeris(eph, &msg);
         sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *)&msg);
       }
     }
@@ -110,7 +115,7 @@ static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* contex
 
   ephemeris_t e;
   unpack_ephemeris((msg_ephemeris_t *)msg, &e);
-  if (e.sid.sat >= MAX_SATS) {
+  if (!sid_valid(e.sid)) {
     log_warn("Ignoring ephemeris for invalid sat");
     return;
   }
@@ -122,8 +127,8 @@ void ephemeris_setup(void)
 {
   memset(es_candidate, 0, sizeof(es_candidate));
   memset(es, 0, sizeof(es));
-  for (u8 i=0; i<32; i++) {
-    es[i].sid.sat = i;
+  for (u32 i=0; i<NUM_SATS; i++) {
+    es[i].sid = sid_from_index(i);
   }
 
   static sbp_msg_callbacks_node_t ephemeris_msg_node;
@@ -137,3 +142,19 @@ void ephemeris_setup(void)
                     NORMALPRIO-1, nav_msg_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_valid(sid));
+  return &es[sid_to_index(sid)];
+}

src/ephemeris.c

@@ -15,11 +15,12 @@
 
 #include <libswiftnav/constants.h>
 #include <libswiftnav/ephemeris.h>
-
-extern Mutex es_mutex;
-extern ephemeris_t es[MAX_SATS];
+#include <libswiftnav/signal.h>
 
 void ephemeris_setup(void);
+void ephemeris_lock(void);
+void ephemeris_unlock(void);
+ephemeris_t *ephemeris_get(gnss_signal_t sid);
 
 #endif