add baseline solution struct, move flag logic out to piksi_firmware

Author: kovach

include/libswiftnav/dgnss_management.h

@@ -39,6 +39,19 @@ typedef struct {
   ambiguities_t float_ambs;
 } ambiguity_state_t;
 
+typedef struct {
+  /* Baseline estimate */
+  double b[3];
+  /* Number of sdiffs used in the soluton. */
+  u8 num_used;
+  /* bool: fixed or float filter derived */
+  u8 fixed_mode;
+  /* bool: raim available for least squares baseline estimate */
+  u8 raim_available;
+  /* bool: raim used to repair baseline */
+  u8 raim_repair;
+} dgnss_baseline_t;
+
 extern dgnss_settings_t dgnss_settings;
 
 void dgnss_set_settings(double phase_var_test, double code_var_test,
@@ -62,13 +75,10 @@ s8 dgnss_iar_get_single_hyp(double *hyp);
 void dgnss_reset_iar(void);
 void dgnss_init_known_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], double b[3]);
 void dgnss_update_ambiguity_state(ambiguity_state_t *s);
-u8 baseline_flag(s8 ret, bool fixed);
-u8 flag_raim_repaired(u8 flag);
-u8 flag_raim_available(u8 flag);
-u8 flag_fixed_mode(u8 flag);
+void fill_property_flags(s8 ret, bool fixed, dgnss_baseline_t *solution);
 s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
                   const double ref_ecef[3], const ambiguity_state_t *s,
-                  u8 *num_used, double b[3],
+                  dgnss_baseline_t *solution,
                   bool disable_raim, double raim_threshold);
 void measure_amb_kf_b(u8 num_sdiffs, sdiff_t *sdiffs,
                       const double receiver_ecef[3], double *b);

src/dgnss_management.c

@@ -395,36 +395,12 @@ void dgnss_update_ambiguity_state(ambiguity_state_t *s)
   }
 }
 
-/** Formats flag bits for sbp.
- *  bit positions:
- *  -`0`: fixed mode?
- *  -`3`: RAIM available?
- *  -`4`: RAIM repair used?
- *
- *  \param ret return code from baseline()
- *  \param fixed 1 for fixed, 0 for float
- *  \return formatted flag value
- */
-u8 baseline_flag(s8 ret, bool fixed)
-{
-  return (ret == 1) << 4  /* RAIM repair? */
-       | (ret != 2) << 3  /* RAIM available? */
-       | fixed;           /* Fixed mode? */
-}
-
-u8 flag_raim_repaired(u8 flag)
-{
-  return flag & (1 << 4);
-}
-
-u8 flag_raim_available(u8 flag)
-{
-  return flag & (1 << 3);
-}
-
-u8 flag_fixed_mode(u8 flag)
+/* Interpret raim-related content of baseline function return code. */
+void fill_property_flags(s8 ret, bool fixed, dgnss_baseline_t *solution)
 {
-  return flag & 1;
+  solution->raim_repair    = ret == 1;
+  solution->raim_available = ret != 2;
+  solution->fixed_mode = fixed;
 }
 
 /** Finds the baseline using low latency sdiffs.
@@ -442,37 +418,41 @@ u8 flag_fixed_mode(u8 flag)
  * \param b           Output baseline.
  * \param disable_raim Flag to turn off raim checks/repair.
  * \param raim_threshold raim check threshold
- * \return positive value: SBP baseline msg flags
+ * \return 0: solution ok
  *         negative value: baseline error code
  */
 s8 dgnss_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
                   const double ref_ecef[3], const ambiguity_state_t *s,
-                  u8 *num_used, double b[3],
+                  dgnss_baseline_t *solution,
                   bool disable_raim, double raim_threshold)
 {
   s8 ret;
 
   /* Try IAR resolved baseline */
-  ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->fixed_ambs, num_used, b,
-                    disable_raim, raim_threshold);
+  ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->fixed_ambs,
+                 &solution->num_used, solution->b,
+                 disable_raim, raim_threshold);
   if (ret >= 0) {
     if (ret == 1)
       log_warn("dgnss_baseline: Fixed baseline RAIM repair");
     log_debug("fixed solution");
     DEBUG_EXIT();
-    return baseline_flag(ret, true);
+    fill_property_flags(ret, true, solution);
+    return 0;
   }
 
   /* We weren't able to get an IAR resolved baseline, check if we can get a
    * float baseline. */
-  ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->float_ambs, num_used, b,
+  ret = baseline(num_sdiffs, sdiffs, ref_ecef, &s->float_ambs,
+                 &solution->num_used, solution->b,
                  disable_raim, raim_threshold);
   if (ret >= 0) {
     if (ret == 1)
       log_warn("dgnss_baseline: Float baseline RAIM repair");
     log_debug("float solution");
     DEBUG_EXIT();
-    return baseline_flag(ret, false);
+    fill_property_flags(ret, false, solution);
+    return 0;
   }
   log_debug("no baseline solution");
   DEBUG_EXIT();

tests/check_dgnss_management.c

@@ -5,6 +5,7 @@
 #include "check_utils.h"
 #include "dgnss_management.h"
 #include "ambiguity_test.h"
+#include "printing_utils.h"
 #include "amb_kf.h"
 #include "printing_utils.h"
 
@@ -205,41 +206,38 @@ START_TEST(test_dgnss_baseline_1)
     }
   };
 
-  double b[3];
-  u8 num_used;
+  dgnss_baseline_t solution;
 
   /* Float only */
   s.fixed_ambs.n = 0;
-  s8 valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b,
+  s8 valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution,
     false, DEFAULT_RAIM_THRESHOLD);
   /* baseline_flag(initial solution ok, float mode) */
-  fail_unless(valid == baseline_flag(0, false));
-  fail_unless(num_used == 5);
-  fail_unless(within_epsilon(b[0], -0.742242));
-  fail_unless(within_epsilon(b[1], -0.492905));
-  fail_unless(within_epsilon(b[2], -0.0533294));
+  fail_unless(valid == 0);
+  fail_unless(solution.fixed_mode == 0);
+  fail_unless(solution.num_used == 5);
+  double expected1[] = {-0.742242, -0.492905, -0.0533294};
+  fail_unless(arr_within_epsilon(3, solution.b, expected1));
 
   /* Fixed and float */
   s.fixed_ambs.n = 4;
-  valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b,
+  valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution,
     false, DEFAULT_RAIM_THRESHOLD);
   /* baseline_flag(initial solution ok, fixed mode) */
-  fail_unless(valid == baseline_flag(0, true));
-  fail_unless(num_used == 5);
-  fail_unless(within_epsilon(b[0], -0.417486));
-  fail_unless(within_epsilon(b[1], -0.358386));
-  fail_unless(within_epsilon(b[2],  0.271427));
+  fail_unless(valid == 0);
+  fail_unless(solution.fixed_mode == 1);
+  fail_unless(solution.num_used == 5);
+  double expected2[] = {-0.622609, -0.432371, -0.00461595};
+  fail_unless(arr_within_epsilon(3, solution.b, expected2));
 
   /* No solution possible */
   s.fixed_ambs.n = 0;
   s.float_ambs.n = 0;
-  valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &num_used, b,
+  valid = dgnss_baseline(num_sdiffs, sdiffs, ref_ecef, &s, &solution,
     false, DEFAULT_RAIM_THRESHOLD);
   fail_unless(valid == -1);
-  fail_unless(num_used == 5);
-  fail_unless(within_epsilon(b[0], -0.417486));
-  fail_unless(within_epsilon(b[1], -0.358386));
-  fail_unless(within_epsilon(b[2],  0.271427));
+  fail_unless(solution.num_used == 5);
+  fail_unless(arr_within_epsilon(3, solution.b, expected2));
 }
 END_TEST