ogr2rnx.c

The OpenGPSRec receiver software runs on the real time operating system RTAI-Linux. It compiles with

      trk = rec[6];
      flag = rec[7];
    }
    if (ONE_SAT)
    {
      if (svid != SELECTED_SV)
        continue;
      printf
        ("0x1a ----------------------------------------------------------------\n");
      printf ("PRN %02d: Chann %02d  Elev %2d  Q %5d ", svid + 1, k,
              elev, sq);
      printf ("( Phase ) %5.3f  Trk %d Flag %d\n", frac_phase, trk, flag);
    }
    else
    {
      printf ("  %02d  -> ", k);
      if (svid == 0xff)
        printf ("not used\n");
      else
      {
        printf ("%02d  |  %02d  | %6d |", svid + 1, elev, sq);
        printf ("  %05.3f |  %2u  |  %2u  |\n", frac_phase, trk, flag);
      }
    }
  }
  // printf("----------------------------------------------------------\n");
  // getch();
}

type_rec0x16
process_0x16 (BYTE * record)
{
  type_rec0x16 rec;
  float dif;

  rec.sv = record[20];
  if (ONE_SAT && rec.sv != SELECTED_SV)
    return rec;

  if (ETREX)
  {
    memcpy (&rec.pr, record, 8);
    memcpy (&rec.delta_pr, record + 8, 4);
    memcpy (&rec.f1, record + 12, 4);
    memcpy (&rec.f2, record + 16, 4);
  }
  else
  {
    memcpy (&rec.delta_pr, record, 4);
    memcpy (&rec.f1, record + 4, 4);
    memcpy (&rec.pr, record + 8, 8);
    memcpy (&rec.f2, record + 16, 4);
  }

  if (VERBOSE)
  {
    printf
      ("0x16 ---------------------------------------------------------\n");
    printf ("PRN %02d: ", rec.sv + 1);
    printf ("Pseudorange %13.3f ", rec.pr);
    printf ("Doppler(m/s)  %7.2f -> %.3f Hz ", rec.delta_pr,
            rec.delta_pr / lambda);
    printf ("F1 %.2f F2 %.2f\n", rec.f1, rec.f2);
    if (ETREX)
      printf ("Unknown %d %d %d\n", record[21], record[22], record[23]);

    if ((DIF_RECORDS) && (ONE_SAT) && (rec.pr == rec38.pr))
    {
      dif = (rec.delta_pr - phase_rate) * 100;
      if (fabs (dif) < 5000)
      {
        mean2 += dif;
        sigma2 += (dif * dif);
        n2++;
        printf ("\t(delta_pr-phase_rate) -> ");
        printf ("Mean %.1f std %.1f\n", mean2 / n2,
                sqrt ((sigma2 - mean2 * mean2 / n2) / n2));
      }
    }
  }

  return rec;
}

BYTE
process_0x39 (BYTE * record)
{
  BYTE sv;

  sv = record[34];
  if (ONE_SAT && (sv != SELECTED_SV))
    return sv;

  mean = sigma = 0.0;
  n = 0;

  if (VERBOSE)
  {
    printf
      ("0x39 ---------------------------------------------------------\n");
    printf ("PRN %02d: ACQUIRED PSEUDORANGE\n", record[34] + 1);
  }

  return sv;
}


ULONG
tobin (BYTE b)
{
  ULONG res;
  BYTE mask = 128;
  int k;

  res = 0;
  for (k = 0, mask = 128; k < 8; k++, mask /= 2)
  {
    res = (res * 10) + ((b & mask) >> (7 - k));
  }

  return res;
}

type_rec0x38
process_0x38 (BYTE * record)
{
  int nsec, doppler;
  double dt, slip, d_pr, drift, d_phase, phase;
  ULONG d_cphase, d_intphase, d_c511;
  type_rec0x38 rec;
  long dtracked;
  long itow, dtow;

  last38 = rec38;

  if (ONE_SAT && (record[36] != SELECTED_SV))
  {
    rec.sv = record[36];
    return rec;
  }

  if (ETREX)
  {
    memcpy (&(rec38.pr), record, 8);
    memcpy (&(rec38.tow), record + 8, 8);
    memcpy (&(rec38.c_phase), record + 16, 4);
    memcpy (&(rec38.tracked), record + 20, 4);
    memcpy (&(rec38.int_phase), record + 24, 4);
    memcpy (&(rec38.c511), record + 28, 4);
    memcpy (&(rec38.delta_f), record + 32, 2);
    memcpy (&(rec38.db), record + 34, 2);
    memcpy (&(rec38.sv), record + 36, 1);
  }
  else
  {
    memcpy (&(rec38.c_phase), record, 4);
    memcpy (&(rec38.tracked), record + 4, 4);
    memcpy (&(rec38.delta_f), record + 8, 2);
    memcpy (&(rec38.int_phase), record + 10, 4);
    memcpy (&(rec38.pr), record + 14, 8);
    memcpy (&(rec38.c511), record + 22, 4);

    memcpy (&(rec38.db), record + 26, 2);
    memcpy (&(rec38.tow), record + 28, 8);
    memcpy (&(rec38.sv), record + 36, 1);
  }

  // rec38.delta_f=*((UINT*)(record+8));
  // rec38.int_phase=*((ULONG*)(record+10));
  // rec38.pr=*((double*)(record+14));
  // rec38.c511=*((ULONG*)(record+22));
  // rec38.db=*((UINT*)(record+26));
  // rec38.tow=*((double*)(record+28));
  // rec38.sv=record[36];

  doppler = rec38.delta_f - 32768;
  phase = rec38.int_phase + (rec38.c_phase & 2047) / 2048.0;

  if (VERBOSE)
  {
    // Check that tow is within limits
    itow = (long) floor (rec38.tow + 0.5);
    if (START == -1)
      START = itow;
    dtow = itow - START;
    if ((itow >= START) && (itow <= LAST) && (dtow < ELAPSED))
    {
      printf
        ("0x38 ----------------------------------------------------------------\n");
      printf ("PRN %02d: Sgn Q %5d ", rec38.sv + 1, rec38.db);
      printf ("TRACKED %08x %010d -> %.3f\n", rec38.tracked,
              (int) rec38.tracked, (float) rec38.tracked / 256.0);
      printf ("\tTOW %14.7f (Counter 0.5 Mhz = %8lu).\n", rec38.tow,
              rec38.c511);
      printf ("\tPseudoRange  %14.3f  Integrated Phase %14.3f\n",
              rec38.pr, phase);

      printf ("\tDoppler(\?): %5d Hz -> %.2f m/s\n", rec38.delta_f,
              rec38.delta_f * lambda);
    }
  }

  if (ONE_SAT && DIF_RECORDS)   // Differencing records is only valid when 
    // tracking one sat
  {
    dt = rec38.tow - last38.tow;
    nsec = floor (dt + 0.499999999);    // GPS time exact increment
    drift = (dt - nsec) / nsec; // Clock drift from the 1''
    // increment
    d_pr = (rec38.pr - last38.pr);      // Pseudorange increment
    d_cphase = rec38.c_phase - last38.c_phase;  // Phase counter
    // increment
    d_phase = d_cphase / 2048.0;        // Phase increment
    d_intphase = (rec38.int_phase - last38.int_phase);  // Integer 
    // phase
    // increment 
    // phase_rate=d_phase*lambda/nsec; 
    // //
    // phase
    // rate
    // (in
    // cm/s)
    d_c511 = rec38.c511 - last38.c511;
    slip = (d_pr - d_phase * lambda) * 100;     // discrepancy between
    // phase and pseudorange
    if (fabs (slip) < 5000)
    {
      mean_q += rec38.db;
      mean += (slip / nsec);
      sigma += (slip * slip / (nsec * nsec));
      n++;
    }
    dtracked = rec38.tracked - last38.tracked;

    if (VERBOSE)
    {
      printf ("Deltas:");
      printf
        (" 0.5Mhz %7ld  Clk drift: %.1f musec/sec -> (m/s) %.1f\n",
         d_c511, drift * 1e6, drift * c);
      printf ("\tdTracked %8d\n", dtracked);
      printf
        ("\tdPR(m) %.1f dintPhase %u  dPhase %.3f dPR(m/s) %.1f\n",
         d_pr, d_intphase, d_phase, d_pr / dt);
      printf ("\td_PR-d_PHASE %.0f cm ", slip);
      printf ("(mean %.1f, std %.1f) ", mean / n,
              sqrt ((sigma - mean * mean / n) / n));
      printf ("Mean Q %.0f\n", mean_q / n);
    }
  }

//  printf ("msg 0x38, PRN = %d\n", rec38.sv+1);

  return rec38;
}



void
collect_stats (FILE * org)
{
  BYTE id, L, record[256];
  int k;
  BYTE lengths[256];
  ULONG cont[256];
  BYTE var[256];

  for (k = 0; k < 256; k++)
    cont[k] = var[k] = 0;

  do
  {
    fread (&id, 1, 1, org);
    fread (&L, 1, 1, org);
    fread (record, 1, L, org);  // fseek(org,L,SEEK_CUR); 

    if ((cont[id]) && (lengths[id] != L))
      var[id] = 1;
    cont[id]++;
    lengths[id] = L;
  }
  while (feof (org) == 0);

  if (STDIN == 0)
    fclose (org);


  for (k = 0; k < 256; k++)
  {
    if (cont[k] == 0)
      continue;
    printf ("Record 0x%02x  (%5ld) L=%3d bytes ", k, cont[k], lengths[k]);
    if (var[k])
      printf ("(VAR)");
    printf ("\n");
  }

  exit (0);
  return;
}


void
check (BYTE * record, BYTE L, BYTE type)
{
  int k, size;


  switch (type)
  {
  case AS_BYTE:
    for (k = 0; k < L; k++)
      printf ("%02x ", record[k]);
    printf ("\n");
    break;
  case AS_CHAR:
    for (k = 0; k < L; k++)
      printf ("%c", record[k]);
    printf ("\n");
    break;
  case AS_DOUBLE:
    size = sizeof (double);
    if (L < size)
      return;
    for (k = 0; k <= L - size; k++)
      printf ("%3d %f\n", k, *((double *) (record + k)));
    break;
  case AS_FLOAT:
    size = sizeof (float);
    if (L < size)
      return;
    for (k = 0; k <= L - size; k++)
      printf ("%3d %f\n", k, *((float *) (record + k)));
    break;
  case AS_ULONG:
    size = sizeof (ULONG);
    if (L < size)
      return;
    for (k = 0; k <= L - size; k++)
      printf ("%3d %lu\n", k, *((ULONG *) (record + k)));
    break;
  case AS_UINT:
    size = sizeof (UINT);
    if (L < size)
      return;
    for (k = 0; k <= L - size; k++)
      printf ("%3d %u\n", k, *((UINT *) (record + k)));
    break;
  default:
    break;
  }
  printf
    ("------------------------------------------------------------------------------\n");

}


int
found_in_list (int lista[], BYTE id)
{
  int k = 0;
  int found = 0;

  while (lista[k] != -1)
  {
    if (lista[k] == id)
      found = 1;
    k++;
  }

  return found;
}


void
reset_epoch (rinex_obs list[])
{
  int k;

  for (k = 0; k < 32; k++)
  {
    if (list[k].used != NEVER_USED)
      list[k].used = NO_DUMP;
    list[k].doppler = -1;
  }

}

BYTE
get_q_code (UINT sgn)
{
  // float temp;
  // temp=(float)sgn/2000.0 + 3.0; if (temp>9) temp=9;
  // return (BYTE)floor(temp);

  if (sgn >= 9000)
    return 9;
  if (sgn >= 6000)
    return 8;
  if (sgn >= 4000)
    return 7;
  if (sgn >= 2500)
    return 6;
  return 5;
}



UINT
get_uint (BYTE * ptr)
{
  UINT x;

  x = (UINT) (*ptr++);
  x += 256 * (UINT) (*ptr);

  return x;
}

void
get_prod_id (org, description, prod, version)
     FILE *org;
     char *description;
     UINT *prod;
     float *version;
{
  BYTE id, L, record[256];
  UINT temp, k;
  BOOLEAN bad;

  do
  {
    fread (&id, 1, 1, org);
    fread (&L, 1, 1, org);
    fread (record, 1, L, org);
    switch (id)
    {
    case 0xff:
      bad = 0;
      k = 4;                    // Check if it is a good looking ID record 
      // 
      while (record[k++] && !bad)
        bad = (record[k] >= 128);
      if (!bad)                 // Probably a good ff record
      {
        temp = get_uint (record);
        *prod = temp;
        temp = get_uint (record + 2);
        *version = (float) temp / 100;
        strcpy (description, record + 4);
        rewind (org);
        return;
      }
      else
        break;
    default:
      break;
    }
  }
  while (feof (org) == 0);

  *prod = 0;
  *version = 0.0;
  strcpy (description, "Generic GPS12");
  rewind (org);
  return;
}


void
llh2xyz (double llh[], double xyz[])
{
  double a, f, a_WGS84, f_WGS84;
  double e2, N, temp;
  double lat, lon, h;
  double ellip[2] = { 0, 0 };
  // double ellip[2]={-251.0, -0.14192702}; Hayford (European)

  a_WGS84 = 6378137;
  f_WGS84 = 1 / 298.257223563;
  a = a_WGS84 - ellip[0];
  f = f_WGS84 - ellip[1] * 1e-4;

  e2 = f * (2 - f);

  lat = WGS84_PI * llh[0] / 180;
  lon = WGS84_PI * llh[1] / 180;
  h = llh[2];

  N = a / sqrt (1 - e2 * sin (lat) * sin (lat));

  temp = (N + h) * cos (lat);

  xyz[0] = temp * cos (lon);
  xyz[1] = temp * sin (lon);
  xyz[2] = (N * (1 - e2) + h) * sin (lat);
}


void
get_wdays_and_tow_from_user_date (ULONG * wdays, ULONG * week_secs)
{
  UINT month, day, year, hour, min, sec;
  int gps_week;
  double jd, tow;

  year = (UINT) USER_DATE[0];
  month = (UINT) USER_DATE[1];
  day = (UINT) USER_DATE[2];
  hour = (UINT) 12;
  min = (UINT) 0;
  sec = (UINT) 0;

  jd = julday (year, month, day, hour, min, (double) sec);
  tow = (ULONG) gps_time (jd, &gps_week);
  *week_secs = (ULONG) tow;
  *wdays = (gps_week - 521) * 7;


  if (VERBOSE)
  {
    printf ("%02u/%02u/%4d ", day, month, year);
    printf ("%02d:%02d:%02d -> JD %f :: ", hour, min, sec, jd);
    printf ("GPS Week %4d ToW %6.0f sec  Garmin Weekdays %d\n",
            gps_week, tow, *wdays);
  }

}


void
get_aprox_location_and_wdays_and_tow (org, xyz, wdays, tow)
     FILE *org;
     double xyz[3];
     ULONG *wdays;
     ULONG *tow;
{
  BYTE id, L, record[256];
  type_rec0x33 rec;
  type_rec0x11 rec11;
  type_rec0x0e rec0e;
  int fix = 0;
  int k = 0;
  BYTE found_11, found_0e, found_33;

  found_33 = found_11 = found_0e = 0;


  do
  {
    fread (&id, 1, 1, org);
    fread (&L, 1, 1, org);
    fread (record, 1, L, org);

    switch (id)
    {
    case 0x33:
      rec = process_0x33 (record);
      k++;
      if (k > GET_THIS)
        break;
      fix = rec.fix;

      xyz[0] = 180.0 * rec.pos[0] / WGS84_PI;
      xyz[1] = 180.0 * rec.pos[1] / WGS84_PI;
      xyz[2] = rec.altitud - rec.h_ellip;
      llh2xyz (xyz, xyz);

      *wdays = rec.wdays;
      *tow = (ULONG) floor (rec.tow + 0.5);

      found_33 = 1;
      break;

    case 0x0e:
      rec0e = process_0x0e (record);
      found_0e = 1;
      break;

    case 0x11:
      rec11 = process_0x11 (record);
      found_11 = 1;
      break;

    default:
      break;
    }
  }
  while (feof (org) == 0);


  rewind (org);


  if (found_33)                 // Found 0x33 record
  {
    // printf("Obtained date and position from 0x33 record.Fix =
    // %d.\n",fix);