ogsnavdecode.c

C写的用软件无线电实现的GPS模拟程序

    nav->eph.af0    = tmplng * pow( 2.0, -31.0);
//
//   subframe 2
//
    tmpint            = (int)(sf[2][3] & 0xFFFF);
    nav->eph.crs      = tmpint * pow( 2.0, -5.0);
    tmplng            = (long)( (sf[2][4] >> 8) & 0xffff);
    nav->eph.dn       = tmplng * pow( 2.0, -43.0) * pi;
    tmplng            = ((sf[2][4] & 0xFF) << 24) | sf[2][5];
    nav->eph.ma       = tmplng * pow( 2.0, -31.0) * pi;
    tmpint            = (int)( sf[2][6] >> 8);
    nav->eph.cuc      = tmpint * pow( 2.0, -29.0);
    tmpulng           = ((sf[2][6] & 0xFF) << 24) | sf[2][7];
    nav->eph.ety      = tmpulng * pow( 2.0, -33.0);
    tmpint            = (int)( sf[2][8] >> 8);
    nav->eph.cus      = tmpint * pow( 2.0, -29.0);
    tmpulng           = (((sf[2][8] & 0xFF) << 24) | sf[2][9]);
    nav->eph.sqra     = tmpulng * pow( 2.0, -19.0);
    if ( nav->eph.sqra > 0.0) 
      nav->eph.wm     = 19964981.84 / pow( nav->eph.sqra, 3);
    tmpuint           = (int)(sf[2][10] >> 8);
    nav->eph.toe      = tmpuint * 16.0;
//      fif=(sf[2][10] & 0x80) >> 7;
//
// subframe 3
//
    tmpint            = (int)( sf[3][3] >> 8);
    nav->eph.cic      = tmpint * pow( 2.0, -29.0);
    tmpint            = (int)(sf[3][5] >> 8);
    nav->eph.cis      = tmpint * pow( 2.0, -29.0);
    tmplng            = ((sf[3][5] & 0xFF) << 24) | sf[3][6];
    nav->eph.inc0     = tmplng * pow( 2.0, -31.0) * pi;
    tmpulng           = ((sf[3][3] & 0xFF) << 24) | sf[3][4];
    nav->eph.omega0   = tmpulng * pow( 2.0, -31.0) * pi;
    tmpint            = (int)(sf[3][7] >> 8);
    nav->eph.crc      = tmpint * pow( 2.0, -5.0);
    tmplng            = ((sf[3][7] & 0xFF) << 24) | sf[3][8];
    nav->eph.w        = tmplng * pow( 2.0, -31.0) * pi;
    tmplng            = sf[3][9];
    if ( bit_test_long( tmplng, 24)) 
      tmplng |= 0xFF000000L;
    nav->eph.omegadot = tmplng * pow( 2.0, -43.0) * pi;
    tmpint            = (int)( (sf[3][10] & 0xFFFC) >> 2);
    if ( bit_test_long( tmpint, 14)) 
      tmpint |= 0xC000;
    nav->eph.idot     = tmpint * pow( 2.0, -43.0) * pi;
    if ( nav->eph.inc0 != 0.0 && 
         nav->eph.sqra > 5000.0 &&
         nav->eph.ety < .05) 
      nav->eph.valid = 1;
  }
  else
  {
    printf( "parity check failed!\n");
    printf( "p_error[1] = %x\n", p_error[1]);
    printf( "p_error[2] = %x\n", p_error[2]);
    printf( "p_error[3] = %x\n", p_error[3]);
//    getchar();
  }  
  
//
//    ALMANAC DECODE  subframes 4 and 5
//
//    SUBFRAME 4
//
//  Note:  i4page/i5page denotes the satid of subframe4/5
//
//             subframe4 subframe5
//       page    satid    satid
//     ---------------------------
//         1       57        1
//        2-5    25-28     2-5
//         6       57        7
//        8-10   29-32     8-10
//         11      57       11 
//         12      62       12
//       13-15   52-54    13-15
//         16      57       16
//       17-18   55-56    17-18
//       19-20   58-59    19-20
//         21      57       21
//       22-25   60-63      51
//
  if ( p_error[4] == 0 && p_error[5] == 0 && 
       almanac_valid == 0 && almanac_flag == 0)
  {

    nav->sf4how       = (long)( (sf[4][2] & 0xffff80) >> 7);
    nav->sf5how       = (long)( (sf[5][2] & 0xffff80) >> 7);

//    nav->alm.how       = (long)( sf[4][2] >> 2);
//    nav->alm.sfid      = (int)( (sf[4][2] & 0x1c) >> 2);
//    nav->alm.tow       = (long)( (sf[4][2] & 0xffff80) >> 7);
//    nav->alm.asflag    = (int)( (sf[4][2] & 0x20) >> 5);
//    nav->alm.alertflag = (int)( (sf[4][2] & 0x40) >> 6);

    almanac_flag = 1;
//      i4data= sf[4][3] >> 22;
    i4p = (int)((sf[4][3] & 0x3F0000L) >> 16);
    if ( i4p != i4page)
    {
      i4page = i4p;
      if (i4page > 24 && i4page < 33)
      {
        nav->alm.prn       = i4page;
        nav->alm.week      = gps_week;
        tmpuint            = (int)( sf[4][3] & 0x00FFFFL);
        nav->alm.ety       = tmpuint * pow( 2.0, -21.0);
        tmpuint            = (int)( sf[4][4] >> 16);
        nav->alm.toa       = tmpuint * 4096.0;
        tmplng             = sf[4][4] & 0x00FFFFL;
        if (bit_test_long( tmplng, 16)) 
          tmplng |= 0xFFFF0000L;
        nav->alm.inc       = (tmplng * pow( 2.0, -19.0) + 0.3) * pi;
        tmpint             = (int)(sf[4][5] >> 8);
        nav->alm.omegadot  = tmpint * pow( 2.0, -38.0) * pi;
        nav->alm.health    = (int)(sf[4][5] & 0x0000FF);
        tmpulng            = sf[4][6];
        nav->alm.sqra      = tmpulng * pow( 2.0, -11.0);
        if ( nav->alm.sqra > 0.0) 
          nav->alm.w = 19964981.84 / pow( nav->alm.sqra, 3);
        tmplng             = sf[4][7];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->alm.omega0    = tmplng * pow( 2.0, -23.0) * pi;
        tmplng             = sf[4][8];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->alm.w         = tmplng * pow( 2.0, -23.0) * pi;
        tmplng             = sf[4][9];
        if (bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;  
        nav->alm.ma        = tmplng * pow( 2.0, -23.0) * pi;
//          iaaf0=(sf[4][10] >> 13) | (sf[4][10] & 0x1C);  
        tmplng             = (sf[4][10] >> 13) | ((sf[4][10] & 0x1C) >> 2);  // fixed (GB-020217)  
        if ( bit_test_long( tmplng, 11)) 
          tmplng |= 0xFFFFF800L;
        nav->alm.af0       = tmplng * pow( 2.0, -20.0);
        tmplng             = (sf[4][10] | 0xFFE0) >> 5;
        if ( bit_test_long( tmplng, 11)) 
          tmplng |= 0xFFFFF800L;
        nav->alm.af1       = tmplng * pow( 2.0, -38.0);
      }
      else if ( i4page == 55 )
      {
        nav->alm.text_message[0] = (char)( (sf[4][3] & 0x00FF00) >> 8);
        nav->alm.text_message[1] = (char)(  sf[4][3] & 0x0000FF);

        for ( k=1; k<=7; k++)
        {
          nav->alm.text_message[3*k-1] = (char)( sf[4][k+3] >> 16);
          nav->alm.text_message[3*k  ] = (char)( (sf[4][k+3] & 0x00FF00) >>  8);
          nav->alm.text_message[3*k+1] = (char)(  sf[4][k+3] & 0x0000FF);
        }
      }
      else if ( i4page == 56 )
      {
// broadcast ionospheric correction
        tmpchr        = (int)( (sf[4][3] & 0x00FF00) >> 8);
        nav->iono.al0 = tmpchr * pow( 2.0, -30.0);
        tmpchr        = (int)( sf[4][3] & 0x0000FF);
        nav->iono.al1 = tmpchr * pow( 2.0, -27.0);
        tmpchr        = (int)( sf[4][4] >> 16);
        nav->iono.al2 = tmpchr * pow( 2.0, -24.0);
        tmpchr        = (int)( (sf[4][4] & 0x00FF00) >> 8);
        nav->iono.al3 = tmpchr * pow( 2.0, -24.0);
        tmpchr        = (int)( sf[4][4] & 0x0000FF);
        nav->iono.b0  = tmpchr * 2048.0;
        tmpchr        = (int)( sf[4][5] >> 16);
        nav->iono.b1  = tmpchr * 16384.0;
        tmpchr        = (int)( (sf[4][5] & 0x00FF00) >> 8);
        nav->iono.b2  = tmpchr * 65536.0;
        tmpchr        = (int)( sf[4][5] & 0x00FF);
        nav->iono.b3  = tmpchr * 65536.0;
// broadcast UTC data
        tmplng        = sf[4][6];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->utc.a1    = tmplng * pow( 2.0, -50.0);
        tmplng         = (sf[4][7] << 8) | (sf[4][8] >> 16);
        nav->utc.a0    = tmplng * pow( 2.0, -30.0);
        tmpint         = (int)( (sf[4][8] & 0x00FF00) >> 8);
        nav->utc.tot   = tmpint * 4096;
        tmpint         = (int)( sf[4][8] & 0xFF);
        nav->utc.WNt   = tmpint;
        tmpint         = (int)( sf[4][9] >> 16);
        if (tmpint > 128) 
          tmpint |= 0xFF00;
        nav->utc.dtls  = tmpint;
        nav->utc.WNlsf = (int)((sf[4][9] & 0x00FF00) >> 8);
        nav->utc.DN    = (int)(sf[4][9] & 0x0000FF);
//          idtlsf = (int)( sf[4][9] >> 16);
        tmpint         = (int)( sf[4][10] >> 16);   // fixed (GB-020217)
        if ( tmpint > 128) 
          tmpint |= 0xFF00;
        nav->utc.dtlsf = tmpint;
      }
      else if ( i4page == 63 )
      {
        ASV[1]  = (int)( (sf[4][3] & 0x00F000L) >> 12);
        ASV[2]  = (int)( (sf[4][3] & 0x000F00)  >>  8);
        ASV[3]  = (int)( (sf[4][3] & 0x0000F0)  >>  4);
        ASV[4]  = (int)(  sf[4][3] & 0x00000F);
        ASV[5]  = (int)(  sf[4][4] >> 20);
        ASV[6]  = (int)( (sf[4][4] & 0x0F0000L) >> 16);
        ASV[7]  = (int)( (sf[4][4] & 0x00F000)  >> 12);
        ASV[8]  = (int)( (sf[4][4] & 0x000F00)  >>  8);
        ASV[9]  = (int)( (sf[4][4] & 0x0000F0)  >>  4);
        ASV[10] = (int)(  sf[4][4] & 0x00000F);
        ASV[11] = (int)(  sf[4][5] >> 20);
        ASV[12] = (int)( (sf[4][5] & 0x0F0000L) >> 16);
        ASV[13] = (int)( (sf[4][5] & 0x00F000)  >> 12);
        ASV[14] = (int)( (sf[4][5] & 0x000F00)  >>  8);
        ASV[15] = (int)( (sf[4][5] & 0x0000F0)  >>  4);
        ASV[16] = (int)(  sf[4][5] & 0x00000F);
        ASV[17] = (int)(  sf[4][6] >> 20);
        ASV[18] = (int)( (sf[4][6] & 0x0F0000L) >> 16);
        ASV[19] = (int)( (sf[4][6] & 0x00F000)  >> 12);
        ASV[20] = (int)( (sf[4][6] & 0x000F00)  >>  8);
        ASV[21] = (int)( (sf[4][6] & 0x0000F0)  >>  4);
        ASV[22] = (int)(  sf[4][6] & 0x00000F);
        ASV[23] = (int)(  sf[4][7] >> 20);
        ASV[24] = (int)( (sf[4][7] & 0x0F0000L) >> 16);
        ASV[25] = (int)( (sf[4][7] & 0x00F000)  >> 12);
        ASV[26] = (int)( (sf[4][7] & 0x000F00)  >>  8);
        ASV[27] = (int)( (sf[4][7] & 0x0000F0)  >>  4);
        ASV[28] = (int)(  sf[4][7] & 0x00000F);
        ASV[29] = (int)(  sf[4][8] >> 20);
        ASV[30] = (int)( (sf[4][8] & 0x0F0000L) >> 16);
        ASV[31] = (int)( (sf[4][8] & 0x00F000)  >> 12);
        ASV[32] = (int)( (sf[4][8] & 0x000F00)  >>  8);

#if 0
        SVh[25] = (int)( sf[4][8] & 0x00003F);
        if( SVh[25] == 0x3f) 
          nav->gps_alm[25].inc = 0.0;
        SVh[26] = (int)( sf[4][9] >> 18);
        if( SVh[26] == 0x3f) 
          nav->gps_alm[26].inc = 0.0;
        SVh[27] = (int)( (sf[4][9] & 0x03F000L) >> 12);
        if( SVh[27] == 0x3f) 
          nav->gps_alm[27].inc = 0.0;
        SVh[28] = (int)( (sf[4][9] & 0x000FC0) >> 6);
        if( SVh[28] == 0x3f) 
          nav->gps_alm[28].inc = 0.0;
        SVh[29] = (int)( sf[4][9] & 0x00003F);
        if( SVh[29] == 0x3f) 
          nav->gps_alm[29].inc = 0.0;
        SVh[30] = (int)( sf[4][10] >> 18);
        if( SVh[30] == 0x3f) 
          nav->gps_alm[30].inc = 0.0;
        SVh[31] = (int)( (sf[4][10] & 0x03F000L) >> 12);
        if( SVh[31] == 0x3f) 
          nav->gps_alm[31].inc = 0.0;
        SVh[32] = (int)( (sf[4][10] & 0x000FC0) >> 6);
        if( SVh[32] == 0x3f) 
          nav->gps_alm[32].inc = 0.0;
#endif
      }  
    }

//
//    SUBFRAME 5
//
//      i5data= sf[5][3] >> 22;
    i5p = (int)((sf[5][3] & 0x3F0000L) >> 16);
//      chan[ch].page5 = i5p;
    if ( i5page != i5p)
    {
      i5page=i5p;
      if ( i5page == 51 )
      {
#if 0
//       iatoa = (int)((sf[5][3] & 0xFF00) >>8);
//       atoa = iatoa*4096;
        SVh[1] = (int)( sf[5][4] >>18);
        if ( SVh[1] == 0x3f) 
          nav->gps_alm[1].inc = 0.0;
        SVh[2] = (int)( (sf[5][4] & 0x03F000L)>>12);
        if ( SVh[2] == 0x3f) 
          nav->gps_alm[2].inc = 0.0;
        SVh[3] = (int)( (sf[5][4] & 0x000FC0)>>6);
        if ( SVh[3] == 0x3f) 
          nav->gps_alm[3].inc = 0.0;
        SVh[4] = (int)( sf[5][4] & 0x00003F);
        if ( SVh[4] == 0x3f) 
          nav->gps_alm[4].inc = 0.0;
        SVh[5] = (int)( sf[5][5] >>18);
        if ( SVh[5] == 0x3f) 
          nav->gps_alm[5].inc = 0.0;
        SVh[6] = (int)( (sf[5][5] & 0x03F000L)>>12);
        if ( SVh[6] == 0x3f) 
          nav->gps_alm[6].inc = 0.0;
        SVh[7] = (int)( (sf[5][5] & 0x000FC0)>>6);
        if ( SVh[7] == 0x3f) 
          nav->gps_alm[7].inc = 0.0;
        SVh[8] = (int)( sf[5][5] & 0x00003F);
        if ( SVh[8] == 0x3f) 
          nav->gps_alm[8].inc = 0.0;
        SVh[9] = (int)( sf[5][6] >> 18);
        if ( SVh[9] == 0x3f) 
          nav->gps_alm[9].inc = 0.0;
        SVh[10] = (int)( (sf[5][6] & 0x03F000L)>>12);
        if ( SVh[10] == 0x3f) 
          nav->gps_alm[10].inc = 0.0;
        SVh[11] = (int)( (sf[5][6] & 0x000FC0)>>6);
        if ( SVh[11] == 0x3f) 
          nav->gps_alm[11].inc = 0.0;
        SVh[12] = (int)( sf[5][6] & 0x00003F);
        if ( SVh[12] == 0x3f) 
          nav->gps_alm[12].inc = 0.0;
        SVh[13] = (int)( sf[5][7] >>18);
        if ( SVh[13] == 0x3f) 
          nav->gps_alm[13].inc = 0.0;
        SVh[14] = (int)( (sf[5][7] & 0x03F000L)>>12);
        if ( SVh[14] == 0x3f) 
          nav->gps_alm[14].inc = 0.0;
        SVh[15] = (int)( (sf[5][7] & 0x000FC0)>>6);
        if ( SVh[15] == 0x3f) 
          nav->gps_alm[15].inc = 0.0;
        SVh[16] = (int)( sf[5][7] & 0x00003F);
        if ( SVh[16] == 0x3f) 
          nav->gps_alm[16].inc = 0.0;
        SVh[17] = (int)( sf[5][8] >>18);
        if ( SVh[17] == 0x3f) 
          nav->gps_alm[17].inc = 0.0;
        SVh[18] = (int)( (sf[5][8] & 0x03F000L)>>12);
        if ( SVh[18] == 0x3f) 
          nav->gps_alm[18].inc = 0.0;
        SVh[19] = (int)( (sf[5][8] & 0x000FC0)>>6);
        if ( SVh[19] == 0x3f) 
          nav->gps_alm[19].inc = 0.0;
        SVh[20] = (int)( sf[5][8] & 0x00003F);
        if ( SVh[20] == 0x3f) 
          nav->gps_alm[20].inc = 0.0;
        SVh[21] = (int)( sf[5][9] >>18);
        if ( SVh[21] == 0x3f) 
          nav->gps_alm[21].inc = 0.0;
        SVh[22] = (int)( (sf[5][9] & 0x03F000L)>>12);
        if ( SVh[22] == 0x3f) 
          nav->gps_alm[22].inc = 0.0;
        SVh[23] = (int)( (sf[5][9] & 0x000FC0)>>6);
        if ( SVh[23] == 0x3f) 
          nav->gps_alm[23].inc = 0.0;
        SVh[24] = (int)( sf[5][9] & 0x00003F);
        if ( SVh[24] == 0x3f) 
          nav->gps_alm[24].inc = 0.0;
#endif
      }
      else
      {
        nav->alm.prn      = i5page;
        nav->alm.week     = gps_week;
        tmpuint           = (int)(sf[5][3] & 0xFFFF);
        nav->alm.ety      = tmpuint * pow(2.0,-21.0);
        tmpuint           = (int)(sf[5][4] >> 16);
        nav->alm.toa      = tmpuint * 4096.0;
        tmplng            = (int)(sf[5][4] & 0xFFFF);
        nav->alm.inc      = (tmplng * pow(2.0,-19.0) + 0.3)*pi;
        tmpint            = (int)(sf[5][5] >> 8);
        nav->alm.omegadot = tmpint * pow(2.0,-38.0) * pi;
        nav->alm.health   = (int)(sf[5][5] & 0xFF);
        tmpulng           = sf[5][6];
        nav->alm.sqra     = tmpulng * pow(2.0,-11.0);
        if ( nav->alm.sqra > 0.0) 
          nav->alm.w      = 19964981.84 / pow( nav->alm.sqra, 3);
        tmplng = sf[5][7];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->alm.omega0   = tmplng * pow(2.0,-23.0) * pi;
        tmplng            = sf[5][8];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->alm.w        = tmplng * pow(2.0,-23.0) * pi;
        tmplng            = sf[5][9];
        if ( bit_test_long( tmplng, 24)) 
          tmplng |= 0xFF000000L;
        nav->alm.ma       = tmplng * pow(2.0,-23.0) * pi;
        tmplng            = (int)((sf[5][10] >> 13) | (sf[5][10] & 0x1C));
        if ( bit_test_long( tmplng, 11)) 
          tmplng |= 0xF800;
        nav->alm.af0      = tmplng * pow(2.0,-20.0);
        tmplng            = (int)((sf[5][10] & 0xFFE0) >> 5);
        if ( bit_test_long( tmplng, 11)) 
          tmplng |= 0xF800;
        nav->alm.af1      = tmplng * pow(2.0,-38.0);
      }
    }
  }
  return;
}

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