ogr2rnx.c

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

    printf ("%2d    %5.0f        ", SELECTED_SV + 1, (float) mean_q / n);
    printf ("%4.1f  %5.1f   ", mean / n,
            sqrt ((sigma - mean * mean / n) / n));
    printf ("%4.1f  %5.1f\n", mean2 / n2,
            sqrt ((sigma2 - mean2 * mean2 / n2) / n2));
  }
  if (STDIN == 0)
    fclose (fd);
  exit (0);
}



// ////////////////PARSE NAV RELATED FUNCTIONS ///////////////////////////

BYTE current_sat;

typedef struct
{
  BYTE prn;
  double acc;
  BYTE health;
  double af[3];
  double tgd;
  double crs;
  double crc;
  double cuc;
  double cus;
  double cic;
  double cis;
  double M0;
  double dn;
  double ecc;
  double roota;
  double W0;
  double Wdot;
  double i0;
  double idot;
  double w;
  double toc;
  int week;
  double toe;
  double tom;
  int iodc;
  int iode2;
  int iode3;
  int last_iode;
  BYTE L2_code;
  BYTE L2_data;
  BYTE fit_flag;
  UINT aodo;
} EPHEM;

EPHEM eph[32];

double URA_TABLE[16] =
  { 2, 2.8, 4, 5.7, 8, 11.3, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096,
  -1
};



void
dump_eph_ok (FILE * fd)
{
  EPHEM ep = eph[current_sat];
  char msg[700], ch;
  char *ptr = msg;
  // int toc,sec,min,hour;
  double toc;
  ULONG garmin_wdays, week;
  struct tm gmt;
  double frac;
  int k;
  BYTE health;
  double cero = 0.0;
  int out;
  char f1[10], f4[40];

  if (ep.prn == 0)
    return;

  if (VC_format)
    strcpy (f1, "%20.12e");
  else
    strcpy (f1, "%19.12e");

  strcpy (f4, "   ");

  for (k = 0; k < 4; k++)
    strcat (f4, f1);
  strcat (f4, "\n");

  health = ep.health >> 5;

  // Convert toc time
  week = (ULONG) ep.week;
  garmin_wdays = (week - 521) * 7;
  toc = ep.toc;
  frac = toc - floor (toc);
  toc = floor (toc);
  get_current_date (garmin_wdays, (ULONG) toc, &gmt);

  ptr +=
    sprintf (ptr, "%2d %02d %2d ", ep.prn, (1900 + gmt.tm_year) % 100,
             gmt.tm_mon + 1);
  ptr += sprintf (ptr, "%02d %2d %2d", gmt.tm_mday, gmt.tm_hour, gmt.tm_min);
  ptr += sprintf (ptr, "%5.1f", (double) gmt.tm_sec + frac);

  // sec=toc%86400; hour=sec/3600; sec-=hour*3600; min=sec/60;
  // sec-=min*60;
  // ptr+=sprintf(ptr,"TOC %d(%02d:%02d:%02d): ",toc,hour,min,sec);

  for (k = 0; k < 3; k++)
    ptr += sprintf (ptr, f1, ep.af[k]);
  ptr += sprintf (ptr, "\n");

  ptr += sprintf (ptr, f4, (double) ep.iode2, ep.crs, ep.dn, ep.M0);
  ptr += sprintf (ptr, f4, ep.cuc, ep.ecc, ep.cus, ep.roota);
  ptr += sprintf (ptr, f4, ep.toe, ep.cic, ep.W0, ep.cis);
  ptr += sprintf (ptr, f4, ep.i0, ep.crc, ep.w, ep.Wdot);
  ptr +=
    sprintf (ptr, f4, ep.idot, (double) ep.L2_code, (double) week,
             (double) ep.L2_data);
  ptr +=
    sprintf (ptr, f4, ep.acc, (double) health, ep.tgd, (double) (ep.iodc));
  ptr += sprintf (ptr, f4, ep.tom, cero, cero, cero);

  /*
   * for(k=0;k<3;k++) ptr+=sprintf(ptr,"%20.12e",ep.af[k]);
   * ptr+=sprintf(ptr,"\n");
   * 
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",(double)ep.iode2,ep.crs,ep.dn,ep.M0);
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.cuc,ep.ecc,ep.cus,ep.roota);
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.toe,ep.cic,ep.W0,ep.cis);
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.i0,ep.crc,ep.w,ep.Wdot);
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.idot,(double)ep.L2_code,(double)week,(double)ep.L2_data);
   * ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.acc,(double)health,ep.tgd,(double)(ep.iodc));
   * //ptr+=sprintf(ptr," %20.12e\n",ep.tom); ptr+=sprintf(ptr,"
   * %20.12e%20.12e%20.12e%20.12e\n",ep.tom,cero,cero,cero); 
   */


  if (VC_format)
  {
    out = 0;
    k = 0;
    while (ch = msg[k])
    {
      if (ch == 'e')
      {
        msg[k - out] = 'D';
        k++;
        msg[k - out] = msg[k];
        k++;
        out++;
      }
      else
        msg[k - out] = ch;
      k++;
    }
    msg[k - out] = 0;
  }
  else
    for (k = 0; k < (ptr - msg); k++)
      if (msg[k] == 'e')
        msg[k] = 'D';

  // printf("%d\n",strlen(msg));

  fprintf (fd, "%s", msg);

  // getchar();
  return;
}


int
format_double_old (char *dest, double a)
{
  char ptr[32];
  int ND, k, exp, index;
  int pos_coma, pos_e;
  char sgn, first;

  sprintf (ptr, "%+20.12e", a); // printf("Inicio: %s\n",ptr);

  k = 0;
  while (ptr[k] != '.')
    k++;
  pos_coma = k;
  while (ptr[k] != 'e')
    k++;
  pos_e = k + 1;

  exp = atoi (ptr + pos_e);

  first = ptr[pos_coma - 1];
  sgn = ptr[pos_coma - 2];

  if (sgn == '+')
    sgn = ' ';

  index = 0;
  dest[index++] = ' ';
  dest[index++] = sgn;
  dest[index++] = '.';

  if (first == '0')
    ND = 12;
  else
  {
    dest[index++] = first;
    ND = 11;
    exp++;
  }

  for (k = 1; k <= ND; k++)
    dest[index++] = ptr[pos_coma + k];

  index += sprintf (dest + index, "D%+03d", exp);

  // printf("Chars written %d\n",index);
  return index;

}


/*
 *   re-format 1.234E+01 into 0.1234D+01
 */
int
format_double (char *dest, double a, int ND)
{
  char ptr[32];
  int k, exp, index;
  int pos_coma, pos_e;
  char sgn, first;
  BOOLEAN leading_cero = 1;


  sprintf (ptr, "%+20.11e", a);       // printf("Inicio: %s\n",ptr);

  k = 0;
  while (ptr[k] != '.')
    k++;
  pos_coma = k;
  while ((ptr[k] != 'e') && (ptr[k] != 'E'))
    k++;
  pos_e = k + 1;
  exp = atoi (ptr + pos_e);

  first = ptr[pos_coma - 1];
  sgn   = ptr[pos_coma - 2];

  if (sgn == '+')
    sgn = ' ';

  index = 0;
  // 
  if (leading_cero)
  {
    dest[index++] = sgn;
    dest[index++] = '0';
  }
  else
  {
    dest[index++] = ' ';
    dest[index++] = sgn;
  }
  dest[index++] = '.';

  dest[index++] = first;
//  ND = 11;
  exp++;

  for (k = 1; k < ND; k++)
    dest[index++] = ptr[pos_coma + k];

  index += sprintf (dest + index, "D%+03d", exp);

  // printf("Chars written %d\n",index);
  return index;
}



int
write_four (char *ptr, double one, double two, double three, double four)
{
  int index = 0;

  index += sprintf (ptr + index, "   ");
  index += format_double (ptr + index, one, 12);
  index += format_double (ptr + index, two, 12);
  index += format_double (ptr + index, three, 12);
  index += format_double (ptr + index, four, 12);

  index += sprintf (ptr + index, "\n");

  return index;
}

void
dump_eph (FILE * fd)
{
  EPHEM ep = eph[current_sat];
  char msg[700];
  char *ptr = msg;
  // int toc,sec,min,hour;
  double toc;
  ULONG garmin_wdays, week;
  struct tm gmt;
  double frac;
  int k;
  BYTE health;
  double cero = 0.0;

  if (ep.prn == 0)
    return;
    
  health = ep.health >> 5;

  // Convert toc time
  week = (ULONG) ep.week;
  garmin_wdays = (week - 521) * 7;
  toc = ep.toc;
  frac = toc - floor (toc);
  toc = floor (toc);
  get_current_date (garmin_wdays, (ULONG) toc, &gmt);

  ptr +=
    sprintf (ptr, "%2d %02d %2d ", ep.prn, (1900 + gmt.tm_year) % 100,
             gmt.tm_mon + 1);
  ptr += sprintf (ptr, "%02d %2d %2d", gmt.tm_mday, gmt.tm_hour, gmt.tm_min);
  ptr += sprintf (ptr, "%5.1f", (double) gmt.tm_sec + frac);

  // sec=toc%86400; hour=sec/3600; sec-=hour*3600; min=sec/60;
  // sec-=min*60;
  // ptr+=sprintf(ptr,"TOC %d(%02d:%02d:%02d): ",toc,hour,min,sec);

  for (k = 0; k < 3; k++)
    ptr += format_double (ptr, ep.af[k], 12);
  ptr += sprintf (ptr, "\n");


  ptr += write_four (ptr, (double) ep.iode2, ep.crs, ep.dn, ep.M0);
  ptr += write_four (ptr, ep.cuc, ep.ecc, ep.cus, ep.roota);
  ptr += write_four (ptr, ep.toe, ep.cic, ep.W0, ep.cis);
  ptr += write_four (ptr, ep.i0, ep.crc, ep.w, ep.Wdot);
  ptr +=
    write_four (ptr, ep.idot, (double) ep.L2_code, (double) week,
                (double) ep.L2_data);
  ptr +=
    write_four (ptr, ep.acc, (double) health, ep.tgd, (double) (ep.iodc));
  ptr += write_four (ptr, ep.tom, cero, cero, cero);


  fprintf (fd, "%s", msg);

//  printf ("%s\n", msg);
  
  return;

  // getchar();
}

void
reset_frame ( void)
{
  memcpy (frame[current_sat], RESET, 30);
  memcpy (check_frame[current_sat], RESET, 30);
  return;
}

void
p_bits (BYTE b)
{
  int i;
  for (i = 7; i >= 0; i--)
    printf ("%d", (b & (1 << i)) >> i);
}

void
p_nbits (BYTE b, int n)
{
  int i;
  for (i = n - 1; i >= 0; i--)
    printf ("%d", (b & (1 << i)) >> i);
}


/*
 * Move bits 2 to 25 from BYTE org[4] to global frame[][]
 */
void
strip_parity (BYTE * org, ULONG word)
{
  BYTE *dest;
  int k, index;
  BYTE d30;

  d30 = (org[3] >> 6) & 0x1;

  index = word * 3;
  dest = frame[current_sat] + index;


  for (k = 0; k < 3; k++)
  {
    dest[k] = ((org[3 - k] & 0x3f) << 2) + ((org[2 - k] & 0xc0) >> 6);
    if (d30 == 1)
      dest[k] = dest[k] ^ 0xff;
    check_frame[current_sat][index + k] = 1;
  }

  // memcpy(check+index,OK,3);

#ifdef TRACE
  printf ("D30 %d\n", d30);
  for (k = 3; k >= 0; k--)
    printf ("%02x ", org[k]);
  printf ("\n");
  for (k = 3; k >= 0; k--)
    p_bits (org[k]);
  printf ("\n");
  printf ("  ");
  for (k = 0; k < 3; k++)
    p_bits (dest[k]);
  printf ("\n");
#endif

  return;
}

// Extract bit number nbit from byte stream ptr
BYTE
peak_bit (int nbit, BYTE * ptr)
{
  int byte_pos = (nbit >> 3);
  int bit_pos = 7 - (nbit & 0x07);
  BYTE res = (ptr[byte_pos] & (1 << bit_pos)) >> bit_pos;
  return res;
}

// Place bit (0/1) in position nbit of byte stream dest
void
poke_bit (BYTE bit, int nbit, BYTE * dest)
{
  int byte_pos = (nbit >> 3);
  int bit_pos = (nbit & 0x07);
  BYTE mask;
  BYTE org = dest[byte_pos];

  // printf("ORG %d byte %d bit %d\n",org,byte_pos,bit_pos);


  bit_pos = 7 - bit_pos;


  mask = 0xff - (1 << bit_pos);
  org &= mask;

  // p_bits(org); printf("\n");
  // p_bits(mask); printf("\n");

  bit &= 0x01;
  org = org | (bit << bit_pos);

  // p_bits(org); printf("\n");

  dest[byte_pos] = org;

  return;
}


// Extract L bits starting from frame[ini-1] as unsigned long int (32 bits 
// max)
ULONG
extrae_ulong (int ini, int L)
{
  ULONG res = 0;
  BYTE *dest = (BYTE *) & res;
  int k, pos_lsb;
  BYTE data;

  ini = ini - 1;
  pos_lsb = ini + L - 1;
  for (k = 0, pos_lsb = ini + L - 1; k < L; k++, pos_lsb--)
  {
    data = peak_bit (pos_lsb, frame[current_sat]);
    poke_bit (data, LSB[k], dest);
  }
  return res;
}



double
get_real (ULONG data, int L, BYTE signo, int scale)
{
  double res, factor;
  ULONG limit;

  res = (double) data;
  if (signo)
  {
    limit = 1 << (L - 1);
    if (data >= limit)
      res = res - pow ((double) 2.0, (double) L);
  }

  factor = pow (2.0, (double) scale);
  res = res * factor;


  return res;
}

void
fill_subframe1 (void)
{
  ULONG temp;
  double d;
  int L;
  BYTE alert, antispoof;
  char on_off[2][4];
  char *tab = "        ";

  strcpy (on_off[0], " ON");
  strcpy (on_off[1], "OFF");

  eph[current_sat].prn = current_sat + 1;


  alert = (BYTE) extrae_ulong (42, 1);
  antispoof = (BYTE) extrae_ulong (43, 1);


  // Possible rollover problems ca. 2020
  temp = extrae_ulong (49, 10);
  eph[current_sat].week = temp + 1024;
  temp = extrae_ulong (59, 2);
  eph[current_sat].L2_code = (BYTE) temp;
  temp = extrae_ulong (61, 4);
  eph[current_sat].acc = URA_TABLE[temp];
  temp = extrae_ulong (71, 2);
  temp = (temp << 8) + extrae_ulong (169, 8);
  eph[current_sat].iodc = temp;
  temp = extrae_ulong (73, 1);
  eph[current_sat].L2_data = (BYTE) temp;
  temp = extrae_ulong (65, 6);
  eph[current_sat].health = (BYTE) temp;

  L = 8;
  temp = extrae_ulong (161, L);
  d = get_real (temp, L, 1, -31);
  eph[current_sat].tgd = d;

  temp = extrae_ulong (177, 16);
  temp <<= 4;
  eph[current_sat].toc = temp;

  L = 22;
  temp = extrae_ulong (217, L);
  d = get_real (temp, L, 1, -31);
  eph[current_sat].af[0] = d;

  L = 16;
  temp = extrae_ulong (201, L);
  d = get_real (temp, L, 1, -43);
  eph[current_sat].af[1] = d;

  L = 8;
  temp = extrae_ulong (193, 8);
  d = get_real (temp, L, 1, -55);
  eph[current_sat].af[2] = d;

  if (VERBOSE_NAV == 0)
    return;

  if ((SELECTED_SF != -1) && (SELECTED_SF != 1))
    return;

  // printf("------------------------------------------------------------------\n");
  printf ("\nPRN %02d: ", eph[current_sat].prn);
  printf ("Issue Of Data Clock (IODC) %04u -> IODE %3d. ",
          eph[current_sat].iodc, eph[current_sat].iodc & 0xff);
  printf ("Week