ogsnavencode.c

OpenGPSSim是一个gps模拟程序

  ulong     = (long)( nav->eph.sqra / pow( 2.0, -19.0));  // 32 bits, scale 2^-19
  sf[2][8]  |= ((ulong & 0xFF000000) >> 24);
  sf[2][9]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.toe / 16.0) & 0xFFFF; // 16 bits, scale 2^4
  sf[2][10] |= (ulong << 8);

//
// subframe 3
//
  ulong     = (long)( nav->eph.cic / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29
  sf[3][3]  |= (ulong << 8);

  ulong     = (long)( nav->eph.omega0 / (pow( 2.0, -31.0) * pi));  // 32 bits, scale 2^-31, rad
  sf[3][3]  |= ((ulong & 0xFF000000) >> 24);
  sf[3][4]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.cis / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29
  sf[3][5]  |= (ulong << 8);

  ulong     = (long)( nav->eph.inc0 / (pow( 2.0, -31.0) * pi));  // 32 bits, scale 2^-31, rad
  sf[3][5]  |= ((ulong & 0xFF000000) >> 24);
  sf[3][6]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.crc / 0.03125) & 0xFFFF; // 16 bits, scale 2^-5
  sf[3][7]  |= (ulong << 8);

  ulong     = (long)( nav->eph.w / (pow( 2.0, -31.0) * pi));  // 32 bits, scale 2^-31, rad
  sf[3][7]  |= ((ulong & 0xFF000000) >> 24);
  sf[3][8]  |= (ulong & 0xFFFFFF);

  ulong     = (long)( nav->eph.omegadot / (pow( 2.0, -43.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-43, rad
  sf[3][9]  |= ulong;

  ulong     = (long)( nav->eph.idot / (pow( 2.0, -43.0) * pi)) & 0x3FFF; // 14 bits, scale 2^-43, rad
  sf[3][10] |= (ulong << 2);

//
//    ALMANAC ENCODE  subframes 4 and 5
//
//    SUBFRAME 4
//

  ulong     = i4satid & 0x3F;
  sf[4][3] |= (ulong << 16);

//  if (i4satid > 24 && i4satid < 33)
//  {
  switch ( i4satid)
  {
  case 25:
  case 26:
  case 27:
  case 29:
  case 30:
  case 31:
  case 32:
//
//  almanac data for satellite 25 thru 32 (frame 5, pages 2,3,4,5,7,8,9,10)
//
//    isv = i4satid;
//    nav->alm.prn = i4satid;

    sf[4][3]  |= (long)( nav->alm.ety / pow( 2.0, -21.0)) & 0xFFFF; // 16 bits, scale 2^-21

    ulong     = (long)( nav->alm.toa / 4096.0) & 0xFF; // 8 bits, scale 2^12
    sf[4][4]  |= (ulong << 16);

    sf[4][4]  |= (long)( (nav->alm.inc/pi-0.3) / pow( 2.0, -19.0)) & 0xFFFF; // 16 bits, scale 2^-19

    ulong     = (long)( nav->alm.omegadot / (pow( 2.0, -38.0) * pi)) & 0xFFFF; // 16 bits, scale 2^-38
    sf[4][5] |= (ulong << 8);

    sf[4][5] |= (long)( nav->alm.health) & 0xFF;

    sf[4][6] |= (long)( nav->alm.sqra / pow( 2.0, -11.0));  // 24 bits, scale 2^-11

    sf[4][7] |= (long)( nav->alm.omega0 / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    sf[4][8] |= (long)( nav->alm.w / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    sf[4][9] |= (long)( nav->alm.ma / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    ulong     = (long)( nav->alm.af0 / pow( 2.0, -20.0)) & 0x7FF;  // 11 bits, scale 2^-20
    sf[4][10] |= ((ulong & 0x7) << 2);
    sf[4][10] |= ((ulong & 0x7F8) << 13);

    ulong     = (long)( nav->alm.af1 / pow( 2.0, -38.0)) & 0x7FF;  // 11 bits, scale 2^-38
    sf[4][10] |= (ulong << 5);
    break;

//  else if ( i4satid == 55)

  case 55:
//
//  special messages (22 ASCII chars) (frame 4, page 17)
//
    ulong = (long)( nav->alm.text_message[0]) & 0xFF;  // 8 bits
    sf[4][3] |= (ulong << 8);

    sf[4][3] |= (long)( nav->alm.text_message[1]) & 0xFF;  // 8 bits

    for ( k=1; k<=7; k++)
    {
      ulong = (long)( nav->alm.text_message[3*k-1]) & 0xFF;  // 8 bits
      sf[4][k+3] |= (ulong << 16);
      ulong = (long)( nav->alm.text_message[3*k]) & 0xFF;  // 8 bits
      sf[4][k+3] |= (ulong << 8);
      ulong = (long)( nav->alm.text_message[3*k+1]) & 0xFF;  // 8 bits
      sf[4][k+3] |= (ulong);
    }
    break;

//  else if ( i4satid == 56)

  case 56:
//
//  ionospheric and UTC data (frame 4, page 18)
//
    ulong     = (long)( nav->iono.al0 / pow( 2.0, -30.0)) & 0xFF;  // 8 bits, scale 2^-30
    sf[4][3]  |= (ulong << 8);

    sf[4][3]  |= (long)( nav->iono.al1 / pow( 2.0, -27.0)) & 0xFF; // 8 bits, scale 2^-27

    ulong     = (long)( nav->iono.al2 / pow( 2.0, -24.0)) & 0xFF; // 8 bits, scale 2^-24
    sf[4][4]  |= sf[4][4] | (ulong << 16);

    ulong     = (long)( nav->iono.al3 / pow( 2.0, -24.0)) & 0xFF; // 8 bits, scale 2^-24
    sf[4][4]  |= (ulong << 8);

    sf[4][4] |= (long)( nav->iono.b0 / 2048.) & 0xFF;  // 8 bits, scale 2^11

    ulong     = (long)( nav->iono.b1 / 16384.0) & 0xFF; // 8 bits, scale 2^14
    sf[4][5] |= (ulong << 16);

    ulong     = (long)( nav->iono.b2 / 65536.0) & 0xFF; // 8 bits, scale 2^16
    sf[4][5] |= (ulong << 8);

    ulong     = (long)( nav->iono.b3 / 65536.0) & 0xFF; // 8 bits, scale 2^16
    sf[4][5] |= (ulong);

    ulong     = (long)( nav->utc.a1 / pow( 2.0, -50.0)) & 0xFFFFFF;  // 24 bits, scale 2^-50
    sf[4][6] |= (ulong);

    ulong     = (long)( nav->utc.a0 / pow( 2.0, -30.0));  // 32 bits, scale 2^-30
    sf[4][7] = sf[4][7] | ((ulong & 0xFFFFFF00) >> 8);
    sf[4][8] |= ((ulong & 0xFF) << 16);

    ulong     = (long)( nav->utc.tot / 4096.0) & 0xFF; // 8 bits, scale 2^12
    sf[4][8]  |= (ulong << 8);

    ulong     = (long)( nav->utc.WNt) & 0xFF; // 8 bits, scale 1
    sf[4][8]  |= (ulong);

    ulong     = (long)( nav->utc.dtls) & 0xFF; // 8 bits, scale 1
    sf[4][9]  |= ulong;

    ulong     = (long)( nav->utc.WNlsf) & 0xFF; // 8 bits, scale 1
    sf[4][9]  |= (ulong << 8);

    ulong     = (long)( nav->utc.DN) & 0xFF;  // 8 bits, scale 1
    sf[4][9] |= ulong;

    ulong     = (long)( nav->utc.dtlsf) & 0xFF; // 8 bits, scale 1
    sf[4][9]  |= (ulong << 16);
    break;

//  else if ( i4satid == 63 )
  case 63:
//
//  satellite configuration for 32 satellites (frame 4, page 25)
//
    for ( k=2; k<6; k++)
      sf[4][3] |= (long)( ASV[-1+k]) << (20-(4*k));

    for ( k=0; k<6; k++)
      sf[4][4] |= (long)( ASV[5+k]) << (20-(4*k));

    for ( k=0; k<6; k++)
      sf[4][5] |= (long)( ASV[11+k]) << (20-(4*k));

    for ( k=0; k<6; k++)
      sf[4][6] |= (long)( ASV[17+k]) << (20-(4*k));

    for ( k=0; k<6; k++)
      sf[4][7] |= (long)( ASV[23+k]) << (20-(4*k));

    for ( k=0; k<4; k++)
      sf[4][8] |= (long)( ASV[29+k]) << (20-(4*k));

    sf[4][8] |= (long)( SVh[25]);

    sf[4][9] |= (long)( SVh[26]) << 18;

    sf[4][9] |= (long)( SVh[27]) << 12;

    sf[4][9] |= (long)( SVh[28]) << 6;

    sf[4][9] |=  (long)( SVh[29]);

    sf[4][10] |= (long)( SVh[30]) << 18;

    sf[4][10] |= (long)( SVh[31]) << 12;

    sf[4][10] |= (long)( SVh[32]) << 6;

    break;
    
  default:
//    printf("unknown i4satid = %d\n", i4satid);
//    printf("unknown i5satid = %d\n", i5satid);
//    getchar();
    break;
  }

//
//    SUBFRAME 5
//

  ulong     = i5satid & 0x3F;
  sf[5][3] |= (ulong << 16);

  switch ( i5satid)
  {
  case 51:

//
//  satellite health data for sats 1 thru 24, almanac reference time,
//  and almanac reference week number (frame 5, page 25)
//
    for ( k=0; k<4; k++)
      sf[5][4] |= (long)( SVh[1+k]) << (18-(6*k));

    for ( k=0; k<4; k++)
      sf[5][5] |= (long)( SVh[5+k]) << (18-(6*k));

    for ( k=0; k<4; k++)
      sf[5][6] |= (long)( SVh[9+k]) << (18-(6*k));

    for ( k=0; k<4; k++)
      sf[5][7] |= (long)( SVh[13+k]) << (18-(6*k));

    for ( k=0; k<4; k++)
      sf[5][8] |= (long)( SVh[17+k]) << (18-(6*k));

    for ( k=0; k<4; k++)
      sf[5][9] |= (long)( SVh[21+k]) << (18-(6*k));

    break;  

  default:
//
//  almanac data for satellite 1 thru 24 (frame 5, pages 1-24)
//
//    isv = i5satid;
//    nav->alm.prn = i5satid;

//
//  copied from subframe4, pages 1-24
//
    ssint     = (int)( nav->alm.ety / pow( 2.0, -21.0));  // 16 bits, scale 2^-21
    ulong     = (long)( ssint) & 0xFFFF;
    sf[5][3]  |= (ulong);

    schar     = (char)( nav->alm.toa / 4096.0);  // 8 bits, scale 2^12
    ulong     = (long)( schar) & 0xFF;
    sf[5][4]  |= (ulong << 16);

    ssint     = (int)( (nav->alm.inc/pi - 0.3) / pow( 2.0, -19.0));  // 16 bits, scale 2^-19
    ulong     = (long)( ssint) & 0xFFFF;
    sf[5][4]  |= (ulong);

    ssint     = (int)( nav->alm.omegadot / (pow( 2.0, -38.0) * pi));  // 16 bits, scale 2^-38
    ulong     = (long)( ssint) & 0xFFFF;
    sf[5][5] |= (ulong << 8);

    sf[5][5] |= (long)( nav->alm.health) & 0xFF;

    sf[5][6] |= (long)( nav->alm.sqra / pow( 2.0, -11.0)) & 0xffffff; // 24 bits, scale 2^-11

    sf[5][7] |= (long)( nav->alm.omega0 / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    sf[5][8] |= (long)( nav->alm.w / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    sf[5][9] |= (long)( nav->alm.ma / (pow( 2.0, -23.0) * pi)) & 0xFFFFFF;  // 24 bits, scale 2^-23, rad

    ulong     = (long)( nav->alm.af0 / pow( 2.0, -20.0)) & 0x7FF;  // 11 bits, scale 2^-20
    sf[5][10] |= ((ulong & 0x7) << 2);
    sf[5][10] |= ((ulong & 0x7F8) << 13);

    ulong     = (long)( nav->alm.af1 / pow( 2.0, -38.0)) & 0x7FF;  // 11 bits, scale 2^-38
    sf[5][10] |= (ulong << 5);

    break;
  }

//  print_sf( sf);

//
//  calculate parity and copy data from sf[][], add parity and
//  write to sfp[][]
//
  add_parity( sf, sfp);

//  print_sf( sfp);

//  copy_sf_to_msg( sfp, msg);

  return;
}


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