Merge pull request #305 from ljbade/improve-dgnss-python

Improve dgnss python bindings

Author: ljbade

python/swiftnav/dgnss_management.pyx

@@ -52,10 +52,7 @@ def dgnss_update_(sdiffs, reciever_ecef, disable_raim=False):
   num_sdiffs = len(sdiffs)
   cdef np.ndarray[np.double_t, ndim=1, mode="c"] ref_ecef_ = np.array(reciever_ecef, dtype=np.double)
   cdef sdiff_t sdiffs_[32]
-  cdef sdiff_t s_
-  for (i,sdiff) in enumerate(sdiffs):
-    s_ = (<SingleDiff ?> sdiff)._thisptr
-    memcpy(&sdiffs_[i], &s_, sizeof(sdiff_t))
+  mk_sdiff_array(sdiffs, 32, &sdiffs_[0])
   dgnss_update(num_sdiffs, &sdiffs_[0], &ref_ecef_[0], disable_raim, DEFAULT_RAIM_THRESHOLD)
 
 # def dgnss_rebase_ref_(sdiffs, reciever_ecef, old_prns):

src/dgnss_management.c

@@ -250,12 +250,21 @@ void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3],
                   bool disable_raim, double raim_threshold)
 {
   DEBUG_ENTRY();
-  if (DEBUG) {
-    printf("sdiff[*].prn = {");
-    for (u8 i=0; i < num_sats; i++) {
-      printf("%u, ", sdiffs[i].sid.sat);
+  log_debug("dgnss_update");
+  log_debug("============");
+  log_debug("n: %u", num_sats);
+  log_debug("ecef: %f %f %f", receiver_ecef[0], receiver_ecef[1], receiver_ecef[2]);
+  log_debug("sdiff[*]");
+  for (u8 i=0; i < num_sats; i++) {
+    log_debug("sid: %u", sdiffs[i].sid.sat);
+    log_debug("  pr: %f", sdiffs[i].pseudorange);
+    log_debug("  cp: %f", sdiffs[i].carrier_phase);
+    log_debug("  do: %f", sdiffs[i].doppler);
+    log_debug("  sn: %f", sdiffs[i].snr);
+    log_debug("  pos/vel");
+    for (u8 j=0; j < 3; j++) {
+      log_debug("    %f %f", sdiffs[i].sat_pos[j], sdiffs[i].sat_vel[j]);
     }
-    printf("}\n");
   }
 
   if (num_sats <= 1) {
@@ -368,6 +377,18 @@ s8 dgnss_iar_get_single_hyp(double *dhyp)
  */
 void dgnss_update_ambiguity_state(ambiguity_state_t *s)
 {
+  log_debug("dgnss_update_ambiguity_state");
+  log_debug("============================");
+  log_debug("==BEFORE==");
+  log_debug("fixed:");
+  for (u8 i=0; i < s->fixed_ambs.n; i++){
+    log_debug("  %u: %f", s->fixed_ambs.sids[i].sat, s->fixed_ambs.ambs[i]);
+  }
+  log_debug("float:");
+  for (u8 i=0; i < s->float_ambs.n; i++){
+    log_debug("  %u: %f", s->float_ambs.sids[i].sat, s->float_ambs.ambs[i]);
+  }
+
   /* Float filter */
   /* NOTE: if sats_management.num_sats <= 1 the filter is not updated and
    * nkf.state_dim may not match. */
@@ -394,6 +415,16 @@ void dgnss_update_ambiguity_state(ambiguity_state_t *s)
   } else {
     s->fixed_ambs.n = 0;
   }
+
+  log_debug("==AFTER==");
+  log_debug("fixed:");
+  for (u8 i=0; i < s->fixed_ambs.n; i++){
+    log_debug("  %u: %f", s->fixed_ambs.sids[i].sat, s->fixed_ambs.ambs[i]);
+  }
+  log_debug("float:");
+  for (u8 i=0; i < s->float_ambs.n; i++){
+    log_debug("  %u: %f", s->float_ambs.sids[i].sat, s->float_ambs.ambs[i]);
+  }
 }
 
 /** Finds the baseline using low latency sdiffs.
@@ -420,12 +451,43 @@ s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
                   u8 *num_used, double b[3],
                   bool disable_raim, double raim_threshold)
 {
+
+  log_debug("dgnss_baseline");
+  log_debug("============");
+  log_debug("n: %u", num_sdiffs);
+  log_debug("ecef: %f %f %f", ref_ecef[0], ref_ecef[1], ref_ecef[2]);
+  log_debug("sdiff[*]");
+  for (u8 i=0; i < num_sdiffs; i++) {
+    log_debug("sid: %u", sdiffs[i].sid.sat);
+    log_debug("  pr: %f", sdiffs[i].pseudorange);
+    log_debug("  cp: %f", sdiffs[i].carrier_phase);
+    log_debug("  do: %f", sdiffs[i].doppler);
+    log_debug("  sn: %f", sdiffs[i].snr);
+    log_debug("  pos/vel");
+    for (u8 j=0; j < 3; j++) {
+      log_debug("    %f %f", sdiffs[i].sat_pos[j], sdiffs[i].sat_vel[j]);
+    }
+  }
+  log_debug("amb[*]");
+  log_debug("fixed:");
+  for (u8 i=0; i < s->fixed_ambs.n; i++){
+    log_debug("  %u: %f", s->fixed_ambs.sids[i].sat, s->fixed_ambs.ambs[i]);
+  }
+  log_debug("float:");
+  for (u8 i=0; i < s->float_ambs.n; i++){
+    log_debug("  %u: %f", s->float_ambs.sids[i].sat, s->float_ambs.ambs[i]);
+  }
+
   s8 ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->fixed_ambs, num_used, b,
                     disable_raim, raim_threshold);
   if (ret >= 0) {
     if (ret == 1) /* TODO: Export this rather than just printing */
+    {
       log_warn("dgnss_baseline: Fixed baseline RAIM repair");
+    }
     log_debug("fixed solution");
+    log_debug("ret: %i ", ret);
+    log_debug("fixed baseline: %f %f %f", b[0], b[1], b[2]);
     DEBUG_EXIT();
     return 1;
   }
@@ -435,8 +497,12 @@ s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
                       disable_raim, raim_threshold))
         >= 0) {
     if (ret == 1) /* TODO: Export this rather than just printing */
+    {
       log_warn("dgnss_baseline: Float baseline RAIM repair");
+    }
     log_debug("float solution");
+    log_debug("ret: %i", ret);
+    log_debug("float baseline: %f %f %f", b[0], b[1], b[2]);
     DEBUG_EXIT();
     return 2;
   }