www.pudn.com > opengpssim.zip > ogsnavencode.c
/* ************************************************************************ * * * GPS Simulation * * * * -------------------------------------------------------------------- * * * * Module: ogsnavencode.cpp * * * * Version: 0.1 * * Date: 02.03.02 * * * * Author: G. Beyerle * * * * -------------------------------------------------------------------- * * * * Copyright (C) 2002-2006 Georg Beyerle * * * * This program is free software; you can redistribute it and/or modify * * it under the terms of the GNU General Public License as published by * * the Free Software Foundation; either version 2 of the License, or * * (at your option) any later version. * * * * This program is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * * GNU General Public License for more details. * * * * You should have received a copy of the GNU General Public License * * along with this program; if not, write to the Free Software * * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. * * * * -------------------------------------------------------------------- * * * * The files 'gpsfuncs.cpp', 'gpsrcvr.cpp' and 'gp2021.cpp' are modi- * * fied versions of the files with the same name from Clifford Kelley's * * OpenSourceGPS distribution. The unmodified files can be obtained * * from http://www.home.earthlink.net/~cwkelley * * * * -------------------------------------------------------------------- * * * * Navigation data encoding * * * ************************************************************************ */ /* ******************************* changes ******************************** dd.mm.yy - ************************************************************************ */ /* ------------------------------- includes ------------------------------- */ #include#include #include #include #include #define OSGLIBRY_H #include "ogsdefine.h" #include "ogsstructs.h" #include "ogsextern.h" #include "ogslibrary.h" /* ------------------------------- defines -------------------------------- */ /* ------------------------------- globals -------------------------------- */ /* -------------------------- prototypes (global) ------------------------- */ static int exor_long( unsigned long x); /* ------------------------------ procedures ------------------------------ */ // // printf content of sf[][] // static void print_sf( unsigned long sf[6][11]) { int i, sfr, word; unsigned long scale; for ( sfr=1; sfr<=5; sfr++) { printf( "--- subframe %d ---\n", sfr); for ( word=1; word<=10; word++) { printf( " %08x", sf[sfr][word]); if (( word % 5) == 0) printf( "\n"); } } getchar(); return; } // // copy frame buffer sf[][] to 1500 byte nav msg array // static void copy_sf_to_msg( unsigned long sf[6][11], char msg[]) { int i, j, sfr, word; unsigned long scale; j=0; for ( sfr=1; sfr<=5; sfr++) { for ( word=1; word<=10; word++) { scale = 0x1L << 29; for ( i=0; i<30; i++) { if ( sf[sfr][word] & scale) msg[j] = 1; else msg[j] = -1; scale = scale >> 1; j++; } } } return; } // // xor all 32 bits in a long variable (b31^b30^b29^...^b2^b1^b0) // static int exor_long( unsigned long x) { char i; int res = 0; for ( i=0; i<32; i++) { res = res ^ (x & 0x1); x = x >> 1; } return res; } #if 0 inline double frem( double x, double y) { double res; if ( x >= 0.0) res = fmod( x, y); else res = -fmod( -x, y); return res; } #endif // // add 6 parity bits to eachword in subframes // // based on gpsrcvr.cpp/parity_check() // Clifford Kelley cwkelley@earthlink.net // Copyright (c) 1996-2001 Clifford Kelley // static void add_parity( unsigned long sf[6][11], unsigned long sfp[6][11]) { unsigned long x; int parity; long d29 = 0, d30 = 0; // assume that last 2 bits from previous frame are zero int sfm, word, b1, b2, b3, b4, b5, b6; long mask1 = 0x3b1f3480L, // just bits 6-29 (GPS bits 1-24) mask2 = 0x1d8f9a40L, mask3 = 0x2ec7cd00L, mask4 = 0x1763e680L, mask5 = 0x2bb1f340L, mask6 = 0x0b7a89c0L; for ( sfm = 1; sfm <= 5; sfm++) { for ( word=1; word<=10; word++) { sfp[sfm][word] = sf[sfm][word] << 6; // calculate parity b1 = parity_exor_7to30( d29, sfp[sfm][word] & mask1) << 5; b2 = parity_exor_7to30( d30, sfp[sfm][word] & mask2) << 4; b3 = parity_exor_7to30( d29, sfp[sfm][word] & mask3) << 3; b4 = parity_exor_7to30( d30, sfp[sfm][word] & mask4) << 2; b5 = parity_exor_7to30( d30, sfp[sfm][word] & mask5) << 1; b6 = parity_exor_7to30( d29, sfp[sfm][word] & mask6); parity = b1 + b2 + b3 + b4 + b5 + b6; if ( d30 == 1) sfp[sfm][word] = ~sfp[sfm][word] & 0x03fffffc0L; // add 6 parity bits sfp[sfm][word] |= (long)( parity & 0x3F); // update d29/d30 bits d29 = (parity & 0x2) >> 1; d30 = parity & 0x1; } } return; } // // encode navigation data to array sfp[][] // void encode_navmsg( NAVDATA *nav, unsigned long sfp[6][11]) { char schar; short unsigned int sint; short int ssint; int k, j, sfr, word; int i4satid, i5satid; char uchar; long slong; unsigned long ulong, preamble = 0x8b; unsigned long sf[6][11]; // 5 subframes (1,...,5), // comprising 10 words (1,...,10), i4satid = nav->sf4svid; // subframe 4 SV id (1,...,63) i5satid = nav->sf5svid; // subframe 5 SV id (1,...,63) // clear subframe array for ( sfr=1; sfr<=5; sfr++) for ( word=1; word<=10; word++) sf[sfr][word] = 0; // // copy ephemeris & almanac data to subframe array sf[][] // // // EPHEMERIS ENCODE subframes 1 to 3 // // subframe 1 // // iodc = int(((sf[1][3] & 0x3) <<8 ) | ((sf[1][8] & 0xFF0000L) >> 16)); sf[1][1] |= preamble << 16; sf[2][1] |= preamble << 16; sf[3][1] |= preamble << 16; sf[4][1] |= preamble << 16; sf[5][1] |= preamble << 16; // sf[1][2] |= (nav->sf1how & 0x1ffff) << 7; // sf[2][2] |= (nav->sf2how & 0x1ffff) << 7; // sf[3][2] |= (nav->sf3how & 0x1ffff) << 7; // sf[4][2] |= (nav->sf4how & 0x1ffff) << 7; // sf[5][2] |= (nav->sf5how & 0x1ffff) << 7; sf[1][2] |= (nav->sf1how & 0x3fffff) << 2; sf[2][2] |= (nav->sf2how & 0x3fffff) << 2; sf[3][2] |= (nav->sf3how & 0x3fffff) << 2; sf[4][2] |= (nav->sf4how & 0x3fffff) << 2; sf[5][2] |= (nav->sf5how & 0x3fffff) << 2; ulong = nav->eph.week & 0x3FF; // 10 bits sf[1][3] |= (ulong << 14); ulong = nav->eph.ura & 0xF; // 4 bits sf[1][3] |= (ulong << 8); ulong = nav->eph.health & 0x3F; // 6 bits sf[1][3] |= (ulong << 2); ulong = nav->eph.iodc & 0x300; // 10 bits, 2 MSB sf[1][3] |= (ulong >> 8); ulong = nav->eph.iodc & 0xFF; // 10 bits, 8 LSB sf[1][8] |= (ulong << 16); // nav->eph.iodc = iodc; sf[1][7] |= (long)( nav->eph.tgd / pow( 2.0, -31.0)) & 0xFF; // 8 bits, scale 2^-31 sf[1][8] |= (long)( nav->eph.toc / 16.0) & 0xFFFF; // 16 bits ulong = (long)( nav->eph.af2 / pow( 2.0, -55.0)) & 0xFF; // 8 bits, scale 2^-55 sf[1][9] |= (ulong << 16); sf[1][9] |= (long)( nav->eph.af1 / pow( 2.0, -43.0)) & 0xFFFF; // 16 bits, scale 2^-43 ulong = (long)( nav->eph.af0 / pow( 2.0, -31.0)) & 0x3FFFFF; // 22 bits, scale 2^-31 sf[1][10] |= (ulong << 2); // // subframe 2 // sf[2][3] |= (long)( nav->eph.crs / pow( 2.0, -5.0)) & 0xFFFF; // 16 bits, scale 2^-5 ulong = (long)( nav->eph.dn / (pow( 2.0, -43.0)*pi)) & 0xFFFF; // (Delta n) 16 bits, scale 2^-43, rad sf[2][4] |= (ulong << 8); ulong = (long)( nav->eph.ma / (pow( 2.0, -31.0)*pi)); // 32 bits, scale 2^-31, rad sf[2][4] |= ((ulong & 0xFF000000) >> 24); sf[2][5] |= (ulong & 0xFFFFFF); ulong = (long)( nav->eph.cuc / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29 sf[2][6] |= (ulong << 8); ulong = (long)( nav->eph.ety / pow( 2.0, -33.0)); // 32 bits, scale 2^-33 sf[2][6] |= ((ulong & 0xFF000000) >> 24); sf[2][7] |= (ulong & 0xFFFFFF); ulong = (long)( nav->eph.cus / pow( 2.0, -29.0)) & 0xFFFF; // 16 bits, scale 2^-29 sf[2][8] |= (ulong << 8); 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 ----------------------------- */