move flag manipulation to sbp_utils, update for dgnss_baseline_t

swift-nav · Aug 7, 2015 · 02a71bb · 02a71bb
1 parent 557e250
commit 02a71bb
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 28 deletions.
diff --git a/src/sbp_utils.c b/src/sbp_utils.c
@@ -286,5 +286,26 @@ void pack_ephemeris(const ephemeris_t *e, msg_ephemeris_t *msg)
   msg->iode      = e->iode;
 }
 
+u8 sbp_format_baseline_flag(dgnss_baseline_t *solution)
+{
+  return solution->fixed_mode
+         | (solution->raim_available << 3)
+         | (solution->raim_repair << 4);
+}
+
+u8 sbp_format_ecef_flag(dgnss_baseline_t *solution)
+{
+  return (solution->fixed_mode ? 1 : 2)
+         | (solution->raim_available << 3)
+         | (solution->raim_repair << 4);
+}
+
+u8 sbp_format_llh_flag(dgnss_baseline_t *solution)
+{
+  return (solution->fixed_mode ? 1 : 2)
+         | (solution->raim_available << 4)
+         | (solution->raim_repair << 5);
+}
+
 /** \} */
 /** \} */
diff --git a/src/sbp_utils.h b/src/sbp_utils.h
@@ -18,6 +18,7 @@
 #include <libsbp/observation.h>
 #include <libswiftnav/gpstime.h>
 #include <libswiftnav/pvt.h>
+#include <libswiftnav/dgnss_management.h>
 
 void sbp_make_gps_time(msg_gps_time_t *t_out, const gps_time_t *t_in, u8 flags);
 void sbp_make_pos_llh(msg_pos_llh_t *pos_llh, const gnss_solution *soln, u8 flags);
@@ -59,6 +60,10 @@ void unpack_ephemeris(const msg_ephemeris_t *msg, ephemeris_t *e);
 
 void pack_ephemeris(const ephemeris_t *e, msg_ephemeris_t *msg);
 
+u8 sbp_format_baseline_flag(dgnss_baseline_t *solution);
+u8 sbp_format_ecef_flag(dgnss_baseline_t *solution);
+u8 sbp_format_llh_flag(dgnss_baseline_t *solution);
+
 /** Value specifying the size of the SBP framing */
 #define SBP_FRAMING_SIZE_BYTES 8
 /** Value defining maximum SBP packet size */

diff --git a/src/solution.c b/src/solution.c
@@ -129,11 +129,15 @@ void solution_send_nmea(gnss_solution *soln, dops_t *dops,
  * for conversion from ECEF to local NED coordinates (meters)
  * \param flags u8 RTK solution flags. 1 if float, 0 if fixed
  */
-void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3],
-                            double ref_ecef[3], u8 flags)
+void solution_send_baseline(const gps_time_t *t, dgnss_baseline_t *solution,
+                            double ref_ecef[3])
 {
   double* base_station_pos;
+  u8 n_sats = solution->num_used;
+  double *b_ecef = solution->b;
+
   msg_baseline_ecef_t sbp_ecef;
+  u8 flags = sbp_format_baseline_flag(solution);
   sbp_make_baseline_ecef(&sbp_ecef, t, n_sats, b_ecef, flags);
   sbp_send_msg(SBP_MSG_BASELINE_ECEF, sizeof(sbp_ecef), (u8 *)&sbp_ecef);
 
@@ -161,21 +165,18 @@ void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3],
 
     vector_add(3, base_station_pos, b_ecef, pseudo_absolute_ecef);
     wgsecef2llh(pseudo_absolute_ecef, pseudo_absolute_llh);
-    u8 fix_mode = (flags & 1) ? NMEA_GGA_FIX_RTK : NMEA_GGA_FIX_FLOAT;
+    u8 fix_mode = (solution->fixed_mode) ? NMEA_GGA_FIX_RTK : NMEA_GGA_FIX_FLOAT;
     /* TODO: Don't fake DOP!! */
     nmea_gpgga(pseudo_absolute_llh, t, n_sats, fix_mode, 1.5);
-    /* now send pseudo absolute sbp message */
-    /* TODO: Derive these flag functions from the yaml and remove hardcoding */
+
+    /* now send pseudo absolute sbp messages */
     msg_pos_llh_t pos_llh;
-    u8 llh_flag = (flag_fixed_mode(flags) ? 1 : 2)
-                | (flag_raim_available(flags) << 4)
-                | (flag_raim_repaired(flags) << 5);
-    u8 ecef_flag = (flag_fixed_mode(flags) ? 1 : 2)
-                 | (flag_raim_available(flags) << 3)
-                 | (flag_raim_repaired(flags) << 4);
+    u8 llh_flag = sbp_format_llh_flag(solution);
     sbp_make_pos_llh_vect(&pos_llh, pseudo_absolute_llh, t, n_sats, llh_flag);
     sbp_send_msg(SBP_MSG_POS_LLH, sizeof(pos_llh), (u8 *) &pos_llh);
+
     msg_pos_ecef_t pos_ecef;
+    u8 ecef_flag = sbp_format_ecef_flag(solution);
     sbp_make_pos_ecef_vect(&pos_ecef, pseudo_absolute_ecef, t, n_sats, ecef_flag);
     sbp_send_msg(SBP_MSG_POS_ECEF, sizeof(pos_ecef), (u8 *) &pos_ecef);
   }
@@ -185,32 +186,27 @@ void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3],
 static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
                             const gps_time_t *t)
 {
-  double b[3];
-  u8 num_used, flags;
+  dgnss_baseline_t solution;
   s8 ret;
 
   switch (dgnss_filter) {
   default:
   case FILTER_FIXED:
     chMtxLock(&amb_state_lock);
     ret = dgnss_baseline(num_sdiffs, sdiffs, position_solution.pos_ecef,
-                         &amb_state, &num_used, b,
+                         &amb_state, &solution,
                          disable_raim, DEFAULT_RAIM_THRESHOLD);
     chMtxUnlock();
     if (ret < 0) {
       log_warn("dgnss_baseline returned error: %d", ret);
       return;
     }
-
-    /* dgnss_baseline return value is sbp baseline flag format,
-     * if non-negative. */
-    flags = ret;
     break;
 
   case FILTER_FLOAT:
     chMtxLock(&amb_state_lock);
     ret = baseline(num_sdiffs, sdiffs, position_solution.pos_ecef,
-                   &amb_state.float_ambs, &num_used, b,
+                   &amb_state.float_ambs, &solution.num_used, solution.b,
                    disable_raim, DEFAULT_RAIM_THRESHOLD);
     chMtxUnlock();
     if (ret == 1)
@@ -221,11 +217,11 @@ static void output_baseline(u8 num_sdiffs, const sdiff_t *sdiffs,
       return;
     }
     /* baseline_flag(baseline return code, float mode); */
-    flags = baseline_flag(ret, false);
+    fill_property_flags(ret, false, &solution);
     break;
   }
 
-  solution_send_baseline(t, num_used, b, position_solution.pos_ecef, flags);
+  solution_send_baseline(t, &solution, position_solution.pos_ecef);
 }
 
 void send_observations(u8 n, gps_time_t *t, navigation_measurement_t *m)
@@ -334,12 +330,12 @@ static void solution_simulation()
       simulation_enabled_for(SIMULATION_MODE_RTK)) {
 
     /* raim is "unavailable" */
-    u8 flags = simulation_enabled_for(SIMULATION_MODE_RTK) ? 1 : 0;
+    dgnss_baseline_t solution;
+    solution.fixed_mode = simulation_enabled_for(SIMULATION_MODE_RTK);
+    solution.raim_available = 0;
+    solution.raim_repair = 0;
 
-    solution_send_baseline(&(soln->time),
-      simulation_current_num_sats(),
-      simulation_current_baseline_ecef(),
-      simulation_ref_ecef(), flags);
+    solution_send_baseline(&(soln->time), &solution, simulation_ref_ecef());
 
     double t_check = expected_tow * (soln_freq / obs_output_divisor);
     if (fabs(t_check - (u32)t_check) < TIME_MATCH_THRESHOLD) {

diff --git a/src/solution.h b/src/solution.h
@@ -18,6 +18,7 @@
 #include <libswiftnav/pvt.h>
 #include <libswiftnav/track.h>
 #include <libswiftnav/gpstime.h>
+#include <libswiftnav/dgnss_management.h>
 
 typedef enum {
   SOLN_MODE_LOW_LATENCY,
@@ -47,8 +48,8 @@ void solution_send_sbp(gnss_solution *soln, dops_t *dops);
 void solution_send_nmea(gnss_solution *soln, dops_t *dops,
                         u8 n, navigation_measurement_t *nm,
                         u8 fix_type);
-void solution_send_baseline(const gps_time_t *t, u8 n_sats, double b_ecef[3],
-                            double ref_ecef[3], u8 flags);
+void solution_send_baseline(const gps_time_t *t, dgnss_baseline_t *solution,
+                            double ref_ecef[3]);
 void solution_setup(void);
 
 #endif