ogr_navdecode.c

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

void navmsg_decode_sf1( int ch)
{
  INT16 prn;
  unsigned long *sf;   // subframe

  prn = Chan[ch].prn;
  sf  = Msg[ch].subframe;

#if defined(DUMPDATAIQ) && 0
  static FILE *fpout = NULL;
#endif

//
//  copy nav bits elements (msg[ch].idx,...,msg[ch].idx+1499)
//  gpsbit1/word1/sf1,...,gpsbit30/word10/sf5 to
//  to 5x10 32bit long words sf[6][11]; 
//  bit 31 and 30 contain parity bits d29 and d30 from 
//  previous word; last two parity bits from word 10
//  (and word 2) are zero always!
//

//  printf(" ch=%d, sf[2]=%ld\n", ch, ((sf[2] >> 7) & 0x1ffff));

  if ( FpDmp)
  {
    unsigned long tow_sf;  // tow of this subframe

    tow_sf = ((sf[2] >> 7) & 0x1ffff) * 6;

    write_frames_to_file( FpDmp, ch, 1, tow_sf, Msg[ch].subframe_parity);
  }

#if 0
/*
 *    assemble the bits into subframes and words
 */
  d_toe = param->clock_tow - GPS_Eph[prn].toe;

  if ( d_toe > 302400.0) 
    d_toe -= 604800.0;
  else if ( d_toe < -302400.0)
    d_toe += 604800.0;
#endif

/*
 *  subframe 1 (ephemeris data)
 */
  GPS_Eph[prn].week   = (INT16)(  sf[3] >> 14);
  GPS_Eph[prn].ura    = (INT16)(( sf[3] >> 8 ) & 0xf);
  GPS_Eph[prn].health = (INT16)(( sf[3] >> 2 ) & 0x3f);
  GPS_Eph[prn].iodc   = (INT16)(((sf[3] & 0x3) << 8 ) | 
                                ((sf[8] >> 16 ) & 0xff));
  GPS_Eph[prn].tgd    = ((signed char)(sf[7] & 0xff)) * CONST_2m31;
  GPS_Eph[prn].toc    = ((UINT16)(sf[8] & 0xffff)) * 16.0;
  GPS_Eph[prn].af2    = ((signed char)(sf[9] >> 16)) * CONST_2m55;
  GPS_Eph[prn].af1    = ((INT16)(sf[9] & 0xffff)) * CONST_2m43;
  GPS_Eph[prn].af0    = ((signed long)((sf[10] << 8) & 0xfffffc00L)) * 
                          CONST_2m31 / CONST_2p10;

//  *** FIXME ***
  Ctrl.gps_week       = GPS_Eph[prn].week + 1024;

  return;
}  

/*
 *  decode subframe 2
 */
void navmsg_decode_sf2( int ch)
{
  INT16 prn;
  unsigned long *sf;   // subframe
  
  prn = Chan[ch].prn;
  sf  = Msg[ch].subframe;

//  printf(" ch=%d, sf[2]=%ld\n", ch, ((sf[2] >> 7) & 0x1ffff));

  if ( FpDmp)
  {
    unsigned long tow_sf;  // tow of this subframe

    tow_sf = ((sf[2] >> 7) & 0x1ffff) * 6;

    write_frames_to_file( FpDmp, ch, 2, tow_sf, Msg[ch].subframe_parity);
  }

//
//   subframe 2
//
  GPS_Eph[prn].iode = (INT16)(sf[3] >> 16);

  GPS_Eph[prn].crs  = ((INT16)(sf[3] & 0xffff)) * CONST_2m5;  // signed 16 bit!
  GPS_Eph[prn].dn   = ((INT16)(sf[4] >> 8)) * CONST_2m43 * PI;
  GPS_Eph[prn].ma   = ((signed long)(((sf[4] & 0xff) << 24) | 
                         sf[5])) * CONST_2m31 * PI;
  GPS_Eph[prn].cuc  = ((INT16)(sf[6] >> 8)) * CONST_2m29;   // signed 16 bit!
  GPS_Eph[prn].ety  = ((unsigned long)(((sf[6] & 0xff) << 24) | 
                         sf[7])) * CONST_2m33;
  GPS_Eph[prn].cus  = ((INT16)(sf[8] >> 8)) * CONST_2m29;   // signed 16 bit!
  GPS_Eph[prn].sqra = ((unsigned long)(((sf[8] & 0xff) << 24) | 
                         sf[9])) * CONST_2m19;

  if ( GPS_Eph[prn].sqra > 0.0) 
    GPS_Eph[prn].n0 = SQRTGM / pow( GPS_Eph[prn].sqra, 3.0);

  GPS_Eph[prn].toe  = ((UINT16)(sf[10] >> 8)) * 16.0;

  return;
}  

/*
 *  decode subframe 3
 */
void navmsg_decode_sf3( int ch)
{
  INT16 prn;

//  static INT16 i4page, i5page;
  unsigned long *sf;   // subframe

  prn = Chan[ch].prn;
  sf  = Msg[ch].subframe;

//  printf(" ch=%d, sf[2]=%ld\n", ch, ((sf[2] >> 7) & 0x1ffff));

  if ( FpDmp)
  {
    unsigned long tow_sf;  // tow of this subframe

    tow_sf = ((sf[2] >> 7) & 0x1ffff) * 6;

    write_frames_to_file( FpDmp, ch, 3, tow_sf, Msg[ch].subframe_parity);
  }

//
// subframe 3
//
  GPS_Eph[prn].cic  = ((INT16)(sf[3] >> 8)) * CONST_2m29;   // signed 16 bit!
  GPS_Eph[prn].cis  = ((INT16)(sf[5] >> 8)) * CONST_2m29;
  GPS_Eph[prn].inc0 = ((signed long)(((sf[5] & 0xff) << 24) | 
                             sf[6])) * CONST_2m31 * PI;
  GPS_Eph[prn].W0   = ((signed long)(((sf[3] & 0xff) << 24) | 
                             sf[4])) * CONST_2m31 * PI;
  GPS_Eph[prn].crc  = ((INT16)((sf[7] >> 8) & 0xffff)) * CONST_2m5;
  GPS_Eph[prn].w    = ((signed long)(((sf[7] & 0xff) << 24) | 
                             sf[8])) * CONST_2m31 * PI;
  GPS_Eph[prn].Wdot = ((signed long)((sf[9] << 8) & 0xffffff00L)) * 
                             CONST_2m43 / CONST_2p8 * PI;
  GPS_Eph[prn].idot = ((signed long)((sf[10] << 16) & 0xfffc0000L)) * 
                             CONST_2m43 / CONST_2p18 * PI;

//  valid if reasonable values
  if ( GPS_Eph[prn].inc0 != 0.0 && 
       GPS_Eph[prn].sqra > 5000.0 &&
       GPS_Eph[prn].ety < 0.05) 
  {
    GPS_Eph[prn].valid = 1;
//    gotoxy(1,24);printf("prn=%d, inc0=%e, sqra=%e, ety=%e\n", 
//      prn, GPS_Eph[prn].inc0, GPS_Eph[prn].sqra, GPS_Eph[prn].ety);
  }
  else  
  {
    GPS_Eph[prn].valid = 0;
#if 0
    gotoxy(1,22+ch);
    printf("prn=%d, inc0=%e, sqra=%e, ety=%e\n", 
      prn, GPS_Eph[prn].inc0, GPS_Eph[prn].sqra, GPS_Eph[prn].ety);
#endif
//    ogr_message("prn=%d, inc0=%e, sqra=%e, ety=%e\n", 
//      prn, GPS_Eph[prn].inc0, GPS_Eph[prn].sqra, GPS_Eph[prn].ety);
  }

#if 0
    gotoxy(1,34+ch);
    printf("prn=%d, inc0=%e, sqra=%e, ety=%e valid=%d\n", 
      prn, GPS_Eph[prn].inc0, GPS_Eph[prn].sqra, GPS_Eph[prn].ety, GPS_Eph[prn].valid);
#endif

  return;
}

/*
 *  decode subframe 4
 */
void navmsg_decode_sf4( int ch)
{
  INT16 i, j, prn, i4page;
  unsigned long *sf;   // subframe

  prn = Chan[ch].prn;
  sf  = Msg[ch].subframe;

//  printf(" ch=%d, sf[2]=%ld\n", ch, ((sf[2] >> 7) & 0x1ffff));

  if ( FpDmp)
  {
    unsigned long tow_sf;  // tow of this subframe

    tow_sf = ((sf[2] >> 7) & 0x1ffff) * 6;

    write_frames_to_file( FpDmp, ch, 4, tow_sf, Msg[ch].subframe_parity);
  }

//
//    ALMANAC DECODE  subframes 4 and 5
//

  i4page = (INT16)((sf[3] >> 16) & 0x3fL);

  Chan[ch].page4 = i4page;

//
//    SUBFRAME 4
//
  switch (i4page)
  {
//  almanac for PRN 25-32
  case 25:
  case 26:
  case 27:
  case 28:
  case 29:
  case 30:
  case 31:
  case 32:
    almanac_decode( i4page, sf);
    break;

//  spares
  case 52:
  case 53:
  case 54:
    break;

//  special text messages
  case 55:

    for ( i=0; i<22; i++)
    {
/*  text_message[0] = sf[3] >>  8
 *  text_message[1] = sf[3] >>  0
 *  text_message[2] = sf[4] >> 16
 *  text_message[3] = sf[4] >>  8
 *  text_message[4] = sf[4] >>  0
 *  text_message[5] = sf[5] >> 16
 *  text_message[6] = sf[5] >>  8
 *  text_message[7] = sf[5] >>  0
 *       etc.
 */   
      j = i+1;
      GPS_Alm[prn].text_message[i] = (char)((sf[3+j/3] >> (16 - 8*(j%3))) & 0xff);
    }
    break;

// Broadcast Ionosphere Model & UTC Parameters
  case 56:
// bits 24-31 in sf[][] zero!
    Iono.al0  = ((signed char)((sf[3] >> 8) & 0xff)) * CONST_2m30;  // signed 8 bits
    Iono.al1  = ((signed char)(sf[3] & 0xff)) * CONST_2m27;
    Iono.al2  = ((signed char)((sf[4] >> 16) & 0xff)) * CONST_2m24;
    Iono.al3  = ((signed char)((sf[4] >>  8) & 0xff)) * CONST_2m24;
    Iono.b0   = ((signed char)(sf[4] & 0xff)) * 2048.0;
    Iono.b1   = ((signed char)((sf[5] >> 16) & 0xff)) * 16384.0;
    Iono.b2   = ((signed char)((sf[5] >>  8) & 0xff)) * 65536.0;
    Iono.b3   = ((signed char)(sf[5] & 0xff)) * 65536.0;

    Utc.a1    = ((signed long)((sf[6] << 8) & 0xffffff00L)) * 
                     CONST_2m50 / CONST_2p8;
    Utc.a0    = ((signed long)((sf[7] << 8) | (sf[8] >> 16))) * CONST_2m30;
    Utc.tot   = ((unsigned char)((sf[8] >> 8) & 0xff)) * CONST_2p12;
    Utc.WNt   = (sf[8] & 0xff);

// *** CHECKME ***: according to doc 'dtls' is unsigned 

    Utc.dtls  = (sf[9] >> 16);
    Utc.WNlsf = ((sf[9] >> 8) & 0xff);
    Utc.DN    = (sf[9] & 0xff);
    Utc.dtlsf = ((signed long)((sf[10] << 8) & 0xff000000L)) / CONST_2p24;

    break;

//  spares?
  case 57:
  case 58:
  case 59:
  case 60:
  case 61:
  case 62:
    break;

//  anti-spoofing and satellite constellation
  case 63:
    for ( i=0; i<32; i++)
    {
      j = i + 2;
      Xmit_Info[i+1].as_and_config = (INT16)
        (sf[3+j/6] >> (20 - 4*(j%6))) & 0xf;
    }

    for ( i=24; i<32; i++)
    {
      j = i - 21;
      Xmit_Info[i+1].health_status = (INT16)
        (sf[8+j/4] >> (18 - 6*(j%4))) & 0x3f;
      if ( Xmit_Info[i+1].health_status == 0x3f) 
        GPS_Alm[i+1].inc = 0.0;
    }

  default:
    break;
  }  

  return;
}  

/*
 *  decode subframe 5
 */
void navmsg_decode_sf5( int ch)
{
  INT16 i, i5page;
  unsigned long *sf;  // subframe of 10 words each, parity bits included

  sf  = Msg[ch].subframe;

//  printf(" ch=%d, sf[2]=%ld\n", ch, ((sf[2] >> 7) & 0x1ffff));

  if ( FpDmp)
  {
    unsigned long tow_sf;  // tow of this subframe

    tow_sf = ((sf[2] >> 7) & 0x1ffff) * 6;

    write_frames_to_file( FpDmp, ch, 5, tow_sf, Msg[ch].subframe_parity);
  }

//
//   subframe 5
//
  i5page = (INT16)((sf[3] >> 16 ) & 0x3f);

  Chan[ch].page5 = i5page;

  switch ( i5page)
  {
  case 51:
//  time of applicability:
//  ToA = (INT16)((sf[3] >> 8 ) & 0xff) * 4096;
    for ( i=0; i<24; i++)
    {
      Xmit_Info[i+1].health_status = (INT16)
        ((sf[4+i/4] >> (18 - 6*(i%4))) & 0x3f);
      if ( Xmit_Info[i+1].health_status == 0x3f) 
        GPS_Alm[i+1].inc = 0.0;
    }
    break;

  default:
//  almanac data
    almanac_decode( i5page, sf);
    break;
  }

  return;
}

/*
 *  decode subframe if there is one
 */
void decode_subframe( INT16 ch)
{
  int par;

//  return if not yet ready or not tracking at all
  if ( Msg[ch].subframe_full == 0 || 
       ( Chan[ch].state != track && Chan[ch].state != track_monosat))
    return;
  
  par = check_parity_subframe( ch);

  if ( par)
  {
    Parity_Failed[ch] += 1;
    return;
  }

//  subframe passed parity check - proceed with decoding

  Parity_Passed[ch] += 1;

  Msg[ch].sf_id = (Msg[ch].subframe[2] >> 2) & 0x7;  // subframe[2] is HOW
        
  switch ( Msg[ch].sf_id)
  {
  case 1:
    navmsg_decode_sf1( ch);
    break;

  case 2:
    navmsg_decode_sf2( ch);
    break;

  case 3:
    navmsg_decode_sf3( ch);
    break;

  case 4:
    navmsg_decode_sf4( ch);
    break;

  case 5:
    navmsg_decode_sf5( ch);
    break;

  default:
//    ogr_message( "ch=%d, sf_id = %d\n", ch, Msg[ch].sf_id);
    break;
  }  // --- switch ( Msg[ch].sf_id) ---

  Msg[ch].subframe_full = 0;

  return;
}  

/* ------------------------------- end of file ---------------------------- */