single diff measurement question (intersection of two structs via prns)

sdiff_empty.c

// Compiles with gcc -O0 single_diff_libswiftnav.c

#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <inttypes.h>

typedef uint8_t u8;
typedef uint16_t u16;

/** Structure representing a GPS time. */
typedef struct __attribute__((packed)) {
  double tow; /**< Seconds since the GPS start of week. */
  u16 wn;     /**< GPS week number. */
} gps_time_t;

typedef struct {
  double pseudorange;
  double carrier_phase;
  double doppler;
  double sat_pos[3];
  double snr;
  double lock_time;
  gps_time_t tot;
  u8 prn;
} navigation_measurement_t;

// Calculate single difference observations
typedef struct {
  double pseudorange;
  double carrier_phase;
  double doppler;
  double sat_pos[3];
  double snr;
  u8 prn;
} sdiff_t;

/** Given given two lists of undifferenced input observations
 * (navigation_measurement_t), nav_meas_1 and nav_meas_2, ordered
 * ascendingly by PRN, constructs the pseudorange, carrier_phase, and
 * doppler sdiffs (single difference observations) using the
 * intersection of their PRNs.
 *
 * Also:
 *  0. SNR in the output is the min of the SNRs of inputs 1 and 2.
 *  1. `sat_pos` and `sat_vel` are taken from input 1.
 *
 * Returns the number of sdiffs in the intersection.
 *
 * \param nav_meas1_len  Number of measurements in `nav_meas1`
 * \oaram nav_meas1      Array of undifferenced observations, sorted by PRN
 * \param nav_meas2_len  Number of measurements in `nav_meas2`
 * \oaram nav_meas2      Array of navigation measurements, sorted by PRN
 * \param sdiff          Single difference observations
 *
 * \return               The number of observations written to `sdiff`
 *
 */
u8 single_diff(u8 nav_meas1_len,
               navigation_measurement_t *nav_meas1,
               u8 nav_meas2_len,
               navigation_measurement_t *nav_meas2,
               sdiff_t *sdiff)
{
  
  
  
  
  
  
  
  
  
  
    return 0;
}

void call() {
  // Fake measurements
  navigation_measurement_t nm1 = {.prn = 1};
  navigation_measurement_t nm2 = {.prn = 2};
  navigation_measurement_t nm3 = {.prn = 3};
  navigation_measurement_t nm4 = {.prn = 4};
    
  // Array of measurements.
  navigation_measurement_t nms_no_match1[2];
  navigation_measurement_t nms_no_match2[2];
    
  // Single differences
  sdiff_t sds_out[3];
  memcpy(&nms_no_match1[0], &nm1, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match1[1], &nm2, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match2[0], &nm1, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match2[1], &nm3, sizeof(navigation_measurement_t));
  
  u8 num_match = single_diff(2, nms_no_match1,
                             2, nms_no_match2,
                             sds_out);
  printf("%d", num_match);
}

int main() {
  call();
  printf("Test!");
}

single_diff_libswiftnav.c

typedef uint8_t u8;

/** Structure representing a GPS time. */
typedef struct __attribute__((packed)) {
    double tow; /**< Seconds since the GPS start of week. */
    u16 wn;     /**< GPS week number. */
} gps_time_t;

typedef struct {
    double raw_pseudorange;
    double pseudorange;
    double carrier_phase;
    double raw_doppler;
    double doppler;
    double sat_pos[3];
    double sat_vel[3];
    double snr;
    double lock_time;
    gps_time_t tot;
    u8 prn;
} navigation_measurement_t;

// Calculate single difference observations
typedef struct {
    double pseudorange;
    double carrier_phase;
    double doppler;
    double sat_pos[3];
    double sat_vel[3];
    double snr;
    u8 prn;
} sdiff_t;

// One of the following two questions. The first one is more
// conceptually heterogenous, but the fundamentals are identical.
//
// - Buro, based on IMH's original question from libswiftnav

/** Given given two lists of undifferenced input observations
 * (navigation_measurement_t), nav_meas_1 and nav_meas_2, ordered
 * ascendingly by PRN, constructs the pseudorange, carrier_phase, and
 * doppler sdiffs (single difference observations) using the
 * intersection of their PRNs.
 *
 * Also:
 *  0. SNR in the output is the min of the SNRs of inputs a and b.
 *  1. `sat_pos` and `sat_vel` are taken from input a.
 *
 * Returns the number of sdiffs in the intersection.
 *
 * \param nav_meas1_len  Number of measurements in `nav_meas1`
 * \oaram nav_meas1      Array of undifferenced observations, sorted by PRN
 * \param nav_meas2_len  Number of measurements in `nav_meas2`
 * \oaram nav_meas2      Array of navigation measurements, sorted by PRN
 * \param sdiff          Single difference observations
 *
 * \return               The number of observations written to `sdiff`
 *
 */
u8 single_diff(u8 nav_meas1_len,
               navigation_measurement_t *nav_meas1,
               u8 nav_meas2_len,
               navigation_measurement_t *nav_meas2,
               sdiff_t *sdiff)
{
  u8 i, j, n = 0;
  /* Loop over nav_meas1 and nav_meas2 and check if a PRN is present
     in both. */
  for (i = 0, j = 0; i < nav_meas1_len && j < nav_meas1_len; i++, j++) {
    if (nav_meas1[i].prn < nav_meas2[j].prn)
      j--;
    else if (nav_meas1[i].prn > nav_meas2[j].prn)
      i--;
    else {
      sdiff[n].prn = nav_meas1[i].prn;
      sdiff[n].pseudorange = nav_meas1[i].raw_pseudorange - nav_meas2[j].raw_pseudorange;
      sdiff[n].carrier_phase = nav_meas1[i].carrier_phase - nav_meas2[j].carrier_phase;
      sdiff[n].doppler = nav_meas1[i].raw_doppler - nav_meas2[j].raw_doppler;
      sdiff[n].snr = MIN(nav_meas1[i].snr, nav_meas2[j].snr);
      memcpy(&(sdiff[n].sat_pos), &(nav_meas1[i].sat_pos), 3*sizeof(double));
      memcpy(&(sdiff[n].sat_vel), &(nav_meas1[i].sat_vel), 3*sizeof(double));
      n++;
    }
  }
  return n;
}

void call() {
  // Fake measurements
  navigation_measurement_t nm1 = {.prn = 1};
  navigation_measurement_t nm2 = {.prn = 2};
  navigation_measurement_t nm3 = {.prn = 3};
  navigation_measurement_t nm4 = {.prn = 4};
    
  // Array of measurements.
  navigation_measurement_t nms_no_match1[2];
  navigation_measurement_t nms_no_match2[2];
    
  // Single differences
  sdiff_t sds_out[3];
  memcpy(&nms_no_match1[0], &nm1, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match1[1], &nm2, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match2[0], &nm1, sizeof(navigation_measurement_t));
  memcpy(&nms_no_match2[1], &nm3, sizeof(navigation_measurement_t));
  
  u8 num_match = single_diff(2, nms_no_match1,
                             2, nms_no_match2,
                             sds_out);
}

typedef struct {
    double pseudorange;
    double carrier_phase;
    double doppler;
    double sat_pos[3];
    double sat_vel[3];
    double snr;
    u8 prn;
} sdiff_t;

/** Constructs the pseudorange, carrier_phase, and doppler differenced
 * sdiffs.  Given two lists of sdiff_t, sdiff1 and sdiff2, ordered
 * ascendingly by PRN, this function constructs a new list of sdiffs
 * whose PRNs are the intersection of the PRNs in lists sdiffs1 and
 * sdiff2, and whose pseudoranges, carrier_phases, and dopplers are
 * the difference sdiff1.foo - sdiff2.foo.
 *
 * It returns the number of sdiffs in the intersection.
 *
 * \param sdiff1_len  The length of the sdiffs1 list.
 * \param sdiffs1     A list of sdiff_t stored ascending by PRN.
 * \param sdiff2_len  The length of the sdiffs2 list.
 * \param sdiffs2     A list of sdiff_t stored ascending by PRN.
 * \param ddiffs      The differenced array
 *                    [sdiff1 - sdiff2 | sdiff1.prn = sdiff2.prn]
 *                    which is also stored ascending by prn.
 */
u8 construct_sdiff_difference(u8 sdiff1_len,
                              sdiff_t *sdiffs1,
                              u8 sdiff2_len,
                              sdiff_t *sdiffs2,
                              sdiff_t *ddiffs)
{

}