Main Page | Modules | Namespace List | Class Hierarchy | Class List | Directories | File List | Namespace Members | Class Members | File Members | Related Pages | Examples | Search for 
src

EngNav.cpp

Go to the documentation of this file.
00001 #pragma ident "$Id: EngNav.cpp 876 2007-11-06 14:16:30Z renfroba $"
00002 
00003 
00004 
00005 //============================================================================
00006 //
00007 //  This file is part of GPSTk, the GPS Toolkit.
00008 //
00009 //  The GPSTk is free software; you can redistribute it and/or modify
00010 //  it under the terms of the GNU Lesser General Public License as published
00011 //  by the Free Software Foundation; either version 2.1 of the License, or
00012 //  any later version.
00013 //
00014 //  The GPSTk is distributed in the hope that it will be useful,
00015 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00017 //  GNU Lesser General Public License for more details.
00018 //
00019 //  You should have received a copy of the GNU Lesser General Public
00020 //  License along with GPSTk; if not, write to the Free Software Foundation,
00021 //  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00022 //  
00023 //  Copyright 2004, The University of Texas at Austin
00024 //
00025 //============================================================================
00026 
00027 //============================================================================
00028 //
00029 //This software developed by Applied Research Laboratories at the University of
00030 //Texas at Austin, under contract to an agency or agencies within the U.S. 
00031 //Department of Defense. The U.S. Government retains all rights to use,
00032 //duplicate, distribute, disclose, or release this software. 
00033 //
00034 //Pursuant to DoD Directive 523024 
00035 //
00036 // DISTRIBUTION STATEMENT A: This software has been approved for public 
00037 //                           release, distribution is unlimited.
00038 //
00039 //=============================================================================
00040 
00041 
00042 
00043 
00044 
00045 
00051 #include <cstdlib>
00052 #include <iostream>
00053 #include <cmath>
00054 #include "EngNav.hpp"
00055 #include "icd_200_constants.hpp"
00056 
00057 #ifdef _MSC_VER
00058 #define LDEXP(x,y) ldexp(x,y)
00059 #else
00060 #define LDEXP(x,y) std::ldexp(x,y)
00061 #endif
00062 
00063 namespace gpstk
00064 {
00066    struct DecodeBits
00067    {
00068       short startBit;
00069       short numBits;
00070    };
00071 
00075    struct DecodeQuant
00076    {
00077       short outIndex;    
00078       short pow2;        
00079       short powPI;       
00080       double scale;      
00081       short signq;       
00082       DecodeBits fmt[2]; 
00083       DecodeQuant *nxtq; 
00084    };
00085 
00088    static DecodeQuant *subframeList[11];
00089 
00091    static double PItab[7] = {0,0,0,0,0,0,0};
00092 
00093    static DecodeQuant formats[] = {
00094       { 0,   0,  0,  1.0L, 0,{ { 1, 8} , { 0, 0} },  0 },     /* Preamble */
00095       { 1,   0,  0,  1.0L, 0,{ { 9, 14}, { 0, 0} },  0 },     /* Message  */
00096       { 2,   0,  0,  6.0L, 0,{ { 31,17}, { 0, 0} },  0 },     /* HOW      */
00097       { 3,   0,  0,  1.0L, 0,{ { 48, 2}, { 0, 0} },  0 },     /* "alert"  */
00098       { 4,   0,  0,  1.0L, 0,{ { 50, 3}, { 0, 0} },  0 },    /* SF ID    */
00099       { 5,   0,  0,  1.0L, 0,{ { 61, 10},{ 0, 0} },  0 },    /* week #   */
00100       { 6,   0,  0,  1.0L, 0,{ { 71,  2},{ 0, 0} },  0}, /* L2 code  */
00101       { 7,   0,  0,  1.0L, 0,{ { 73,  4},{ 0, 0} },  0}, /* accuracy */
00102       { 8,   0,  0,  1.0L, 0,{ { 77,  6},{ 0, 0} },  0}, /* health   */
00103       { 9,  11,  0,  1.0L, 0,{ { 83,  2},{ 211,8}},  0}, /* AODC     */
00104       { 10,   0,  0, 1.0L, 0,{ { 91,  1},{ 0,  0}},  0}, /* L2 P     */
00105       { 11, -31,  0, 1.0L, 1,{ {197,  8},{ 0,  0}},  0}, /* Tgd      */
00106       { 12,   4,  0,  1.0L, 0,{ {219, 16},{ 0,  0}},  0}, /* Toc      */
00107       { 13, -55,  0,  1.0L, 1,{ {241,  8},{ 0,  0}},  0}, /* Af2      */
00108       { 14, -43,  0,  1.0L, 1,{ {249, 16},{ 0,  0}},  0}, /* Af1      */
00109       { 15, -31,  0,  1.0L, 1,{ {271, 22}, {0,  0}},  0}, /* Af0      */
00110          /* Pattern 2 */
00111       {0,   0,  0,  1.0L, 0,{ {  1 , 8},  {0,  0  }},  0 }, /* Preamble */
00112       {1,   0,  0,  1.0L, 0,{ {  9, 14},  { 0, 0  }},  0 }, /* Message  */
00113       {2,   0,  0,  6.0L, 0,{ {  31,17},  { 0, 0  }},  0 }, /* HOW      */
00114       {3,   0,  0,  1.0L, 0,{ {  48, 2},  { 0, 0  }},  0 }, /* "alert"  */
00115       {4,   0,  0,  1.0L, 0,{ { 50,  3},  { 0, 0  }},  0 }, /* SF ID    */
00116       {5,  11,  0,  1.0L, 0,{ {  61, 8 }, { 0, 0  }},  0 }, /* AODE     */
00117       {6,  -5,  0,  1.0L, 1,{ {  69, 16 }, { 0, 0  }},  0 }, /* Crs      */
00118       {7, -43,  1,  1.0L, 1,{ {  91,16 }, { 0, 0  }}, 0 }, /* delta n  */
00119       {8, -31,  1,  1.0L, 1,{ { 107, 8 }, {121,24 }}, 0 }, /* M0       */
00120       {9, -29,  0,  1.0L, 1,{ { 151, 16}, { 0,  0 }}, 0}, /* Cuc      */
00121       {10, -33, 0,  1.0L, 0,{ { 167,  8}, {181,24 }},  0}, /* ecc      */
00122       {11, -29, 0,  1.0L, 1,{ {  211,16}, { 0, 0  }},  0}, /* Cus      */
00123       {12, -19, 0,  1.0L, 0,{ {  227, 8}, {241, 24 }},  0}, /* sqrt(A)  */
00124       {13,   4, 0,  1.0L, 0,{ {  271,16}, { 0,  0 }},  0}, /* Toe      */
00125       {14,   0, 0,  1.0L, 0,{ {  287, 1}, { 0,  0 }},  0}, /* fit init */
00126       {15,   0, 0, 900.0L, 0,{ {  288, 5}, { 0,  0 }},  0}, /* AODO     */
00127          /* Pattern 3 */
00128       {0,   0,  0,  1.0L, 0,{ {  1,  8},  { 0,  0 }},  0}, /* Preamble */
00129       {1,   0,  0,  1.0L, 0,{ {  9, 14},  { 0,  0 }},  0}, /* Message  */
00130       {2,   0,  0,  6.0L, 0,{ {  31,17},  { 0,  0 }},  0}, /* HOW      */
00131       {3,   0,  0,  1.0L, 0,{ {  48, 2},  { 0,  0 }},  0}, /* "alert"  */
00132       {4,   0,  0,  1.0L, 0,{ {  50, 3},  { 0,  0 }},  0}, /* SF ID    */
00133       {5, -29,  0,  1.0L, 1,{ {  61,16},  { 0,  0 }},  0}, /* Cic      */
00134       {6, -31,  1,  1.0L, 1,{ {   77,8},  { 91, 24}},  0}, /* OMEGA0   */
00135       {7, -29,  0,  1.0L, 1,{ { 121,16},  { 0,  0 }},  0}, /* Cis      */
00136       {8, -31,  1,  1.0L, 1,{ { 137, 8},  {151, 24}},  0}, /* i0       */
00137       {9,  -5,  0,  1.0L, 1,{ { 181,16},  { 0,  0 }},  0}, /* Crc      */
00138       {10, -31, 1,  1.0L, 1,{ { 197, 8},  {211, 24}},  0}, /* w        */
00139       {11, -43, 1,  1.0L, 1,{ { 241,24},  { 0,  0 }},  0}, /* OMEGAdot */
00140       {12,  11, 0,  1.0L, 0,{ { 271, 8},  { 0,  0 }},  0}, /* AODE     */
00141       {13, -43, 1,  1.0L, 1,{ { 279,14},  { 0,  0 }},  0}, /* idot     */
00142          /* Pattern 4 */
00143       {0,   0,  0,  1.0L, 0,{ { 1,  8},  { 0,  0}},  0 }, /* Preamble */
00144       {1,   0,  0,  1.0L, 0,{ { 9, 14},  { 0,  0}},   0}, /* Message  */
00145       {2,   0,  0,  6.0L, 0,{ { 31,17},  { 0,  0}},   0}, /* HOW      */
00146       {3,   0,  0,  1.0L, 0,{ { 48, 2},  { 0,  0}},   0}, /* "alert"  */
00147       {4,   0,  0,  1.0L, 0,{ { 50, 3},  { 0,  0}},   0}, /* SF ID    */
00148       {5,   0,  0,  1.0L, 0,{ { 61, 2},  { 0,  0}},   0}, /* Dataflag */
00149       {6,   0,  0,  1.0L, 0,{ { 63, 6},  { 0,  0}},   0}, /* Page ID  */
00150       {7, -21,  0,  1.0L, 0,{ { 69,16},  { 0,  0}},   0}, /* e        */
00151       {8,  12,  0,  1.0L, 0,{ { 91, 8},  { 0,  0}},   0}, /* time ep  */
00152       {9, -19,  1,  1.0L, 1,{ { 99,16},  { 0,  0}},   0}, /* i offset */
00153       {10, -38,  1, 1.0L, 1,{ {121,16},  { 0,  0}},   0}, /* OMEGADOT */
00154       {11,   0,  0, 1.0L, 0,{ {137, 8},  { 0,  0}},   0}, /* Health   */
00155       {12, -11,  0, 1.0L, 0,{ {151,24},  { 0,  0}},  0}, /* SQRT(a)  */
00156       {13, -23,  1, 1.0L, 1,{ {181,24},  { 0,  0}},  0}, /* OMEGA    */
00157       {14, -23,  1, 1.0L, 1,{ {211,24},  { 0,  0}},  0}, /* w        */
00158       {15, -23,  1, 1.0L, 1,{ {241,24},  { 0,  0}},  0}, /* Mean Ano */
00159       {16, -20,  0, 1.0L, 1,{ {271, 8},  {290, 3}},  0}, /* AF0      */
00160       {17, -38,  0, 1.0L, 1,{ {279,11},  { 0,  0}},  0}, /* AF1      */
00161       {18,   0,  0, 1.0L, 0,{ { 0,  0},  { 0,  0}},  0}, /* REF WEEK */
00162       {19,   0,  0, 1.0L, 0,{ { 63, 6},  { 0,  0}},  0}, /* PRN #    */
00163       
00164          /* Pattern 5 */
00165       {0,   0,  0,  1.0L,  0,{ { 1,  8},   { 0,  0}},  0}, /* Preamble */
00166       {1,   0,  0,  1.0L,  0,{ { 9, 14},   { 0,  0}},  0}, /* Message  */
00167       {2,   0,  0,  6.0L,  0,{ {31, 17},   { 0,  0}},  0}, /* HOW      */
00168       {3,   0,  0,  1.0L,  0,{ {48,  2},   { 0,  0}},  0}, /* "alert"  */
00169       {4,   0,  0,  1.0L,  0,{ {50,  3},   { 0,  0}},  0}, /* SF ID    */
00170       {5,   0,  0,  1.0L,  0,{ { 61, 2},   { 0,  0}},  0}, /* Dataflag */
00171       {6,   0,  0,  1.0L,  0,{ { 63, 6},   { 0,  0}},  0}, /* Page ID  */
00172       {7,   0,  0,  1.0L,  0,{ { 77, 8},   { 0,  0}},  0}, /* Refweek  */
00173       {8,   0,  0,  1.0L,  0,{ { 91, 6},   { 0,  0}},  0}, /* SV1 Hlth */
00174       {9,   0,  0,  1.0L,  0,{ { 97, 6},   { 0,  0}},  0}, /* SV2 Hlth */
00175       {10,   0,  0,  1.0L, 0,{ { 103,6},   { 0,  0}},  0}, /* SV3 Hlth */
00176       {11,   0,  0,  1.0L, 0,{ { 109,6},   { 0,  0}},  0}, /* SV4 Hlth */
00177       {12,   0,  0,  1.0L, 0,{ { 121,6},   { 0,  0}},  0}, /* SV5 Hlth */
00178       {13,   0,  0,  1.0L, 0,{ { 127,6},   { 0,  0}},  0}, /* SV6 Hlth */
00179       {14,   0,  0,  1.0L, 0,{ { 133,6},   { 0,  0}},  0}, /* SV7 Hlth */
00180       {15,   0,  0,  1.0L, 0,{ { 139,6},   { 0,  0}},  0}, /* SV8 Hlth */
00181       {16,   0,  0,  1.0L, 0,{ { 151,6},   { 0,  0}},  0}, /* SV9 Hlth */
00182       {17,   0,  0,  1.0L, 0,{ { 157,6},   { 0,  0}},  0}, /* SV10 Hlth*/
00183       {18,   0,  0,  1.0L, 0,{ { 163,6},   { 0,  0}},  0}, /* SV11 Hlth*/
00184       {19,   0,  0,  1.0L, 0,{ { 169,6},   { 0,  0}},  0}, /* SV12 Hlth*/
00185       {20,   0,  0,  1.0L, 0,{ { 181,6},   { 0,  0}},  0}, /* SV13 Hlth*/
00186       {21,   0,  0,  1.0L, 0,{ { 187,6},   { 0,  0}},  0}, /* SV14 Hlth*/
00187       {22,   0,  0,  1.0L, 0,{ { 193,6},   { 0,  0}},  0}, /* SV15 Hlth*/
00188       {23,   0,  0,  1.0L, 0,{ { 199,6},   { 0,  0}},  0}, /* SV16 Hlth*/
00189       {24,   0,  0,  1.0L, 0,{ { 211,6},   { 0,  0}},  0}, /* SV17 Hlth*/
00190       {25,   0,  0,  1.0L, 0,{ { 217,6},   { 0,  0}},  0}, /* SV18 Hlth*/
00191       {26,   0,  0,  1.0L, 0,{ { 223,6},   { 0,  0}},  0}, /* SV19 Hlth*/
00192       {27,   0,  0,  1.0L, 0,{ { 229,6},   { 0,  0}},  0}, /* SV20 Hlth*/
00193       {28,   0,  0,  1.0L, 0,{ { 241,6},   { 0,  0}},  0}, /* SV21 Hlth*/
00194       {29,   0,  0,  1.0L, 0,{ { 247,6},   { 0,  0}},  0}, /* SV22 Hlth*/
00195       {30,   0,  0,  1.0L, 0,{ { 253,6},   { 0,  0}},  0}, /* SV23 Hlth*/
00196       {31,   0,  0,  1.0L, 0,{ { 259,6},   { 0,  0}},  0}, /* SV24 Hlth*/
00197          /* Pattern 6 */
00198       {0,   0,  0,   1.0L, 0, { {  1,   8},{  0, 0}},  0}, /* Preamble */
00199       {1,   0,  0,   1.0L, 0, { {  9,  14},{  0, 0}},  0}, /* Message  */
00200       {2,   0,  0,   6.0L, 0, { {  31, 17},{  0, 0}},  0}, /* HOW      */
00201       {3,   0,  0,   1.0L, 0, { {  48,  2},{  0, 0}},  0}, /* "alert"  */
00202       {4,   0,  0,   1.0L, 0, { {  50,  3},{  0, 0}},  0}, /* SF ID    */
00203       {5,   0,  0,   1.0L, 0, { {  61,  2},{  0, 0}},  0}, /* Dataflag */
00204       {6,   0,  0,   1.0L, 0, { {  63,  6},{  0, 0}},  0}, /* Page ID  */
00205       {7,   0,  0,   1.0L, 0, { {  69, 16},{  0, 0}},  0}, /* Reserved */
00206       {8,   0,  0,   1.0L, 0, { {  91, 24},{  0, 0}},  0}, /* Reserved */
00207       {9,   0,  0,   1.0L, 0, { { 121, 24},{  0, 0}},  0}, /* Reserved */
00208       {10,  0,  0,   1.0L, 0, { { 151, 24},{  0, 0}},  0}, /* Reserved */
00209       {11,  0,  0,   1.0L, 0, { { 181, 24},{  0, 0}},  0}, /* Reserved */
00210       {12,  0,  0,   1.0L, 0, { { 211, 24},{  0, 0}},  0}, /* Reserved */
00211       {13,  0,  0,   1.0L, 0, { { 241,  8},{  0, 0}},  0}, /* Reserved */
00212       {14,  0,  0,   1.0L, 0, { {  249,16},{  0, 0}},  0}, /* Reserved */
00213          /* Pattern 7 */
00214       {0,   0,  0,  1.0L, 0, { { 1,   8}, { 0,  0} }, 0}, /* Preamble */
00215       {1,   0,  0,  1.0L, 0, { { 9,  14}, { 0,  0} }, 0}, /* Message  */
00216       {2,   0,  0,  6.0L, 0, { { 31, 17}, { 0,  0} }, 0}, /* HOW      */
00217       {3,   0,  0,  1.0L, 0, { { 48,  2}, { 0,  0} }, 0}, /* "alert"  */
00218       {4,   0,  0,  1.0L, 0, { { 50,  3}, { 0,  0} }, 0}, /* SF ID    */
00219       {5,   0,  0,  1.0L, 0, { { 61,  2}, { 0,  0} }, 0}, /* Dataflag */
00220       {6,   0,  0,  1.0L, 0, { { 63,  6}, { 0,  0} }, 0}, /* Page ID  */
00221       {7,   0,  0,  1.0L, 0, { { 69, 16}, { 0,  0} }, 0}, /* Reserved */
00222       {8,   0,  0,  1.0L, 0, { { 91, 24}, { 0,  0} }, 0}, /* Reserved */
00223       {9,   0,  0,  1.0L, 0, { {121, 24}, { 0,  0} }, 0}, /* Reserved */
00224       {10,  0,  0,  1.0L, 0, { {151, 24}, { 0,  0} }, 0}, /* Reserved */
00225       {11,  0,  0,  1.0L, 0, { {181, 24}, { 0,  0} }, 0}, /* Reserved */
00226       {12,  0,  0,  1.0L, 0, { {211, 24}, { 0,  0} }, 0}, /* Reserved */
00227       {13,  0,  0,  1.0L, 0, { {241,  8}, { 0,  0} }, 0}, /* Reserved */
00228       {14,  0,  0,  1.0L, 0, { {249, 16}, { 0,  0} }, 0}, /* Reserved */
00229          /* Pattern 8 */
00230       {0,   0,  0,   1.0L, 0,{ { 1,   8},{  0, 0}},  0}, /* Preamble */
00231       {1,   0,  0,   1.0L, 0,{ { 9,  14},{  0, 0}},  0}, /* Message  */
00232       {2,   0,  0,   6.0L, 0,{ { 31, 17},{  0, 0}},  0}, /* HOW      */
00233       {3,   0,  0,   1.0L, 0,{ { 48,  2},{  0, 0}},  0}, /* "alert"  */
00234       {4,   0,  0,   1.0L, 0,{ { 50,  3},{  0, 0}},  0}, /* SF ID    */
00235       {5,   0,  0,   1.0L, 0,{ { 61,  2},{  0, 0}},  0}, /* Dataflag */
00236       {6,   0,  0,   1.0L, 0,{ { 63,  6},{  0, 0}},  0}, /* Page ID  */
00237       {7, -30,  0,   1.0L, 1,{ { 69,  8},{  0, 0}},  0}, /* ALPHA0   */
00238       {8, -27, -1,   1.0L, 1,{ { 77,  8},{  0, 0}},  0}, /* ALPHA1   */
00239       {9, -24, -2,   1.0L, 1,{ { 91,  8},{  0, 0}},  0}, /* ALPHA2   */
00240       {10, -24, -3,  1.0L, 1,{ { 99,  8},{  0, 0}},  0}, /* ALPHA3   */
00241       {11,  11,  0,  1.0L, 1,{ { 107, 8},{  0, 0}},  0}, /* BETA0    */
00242       {12,  14, -1,  1.0L, 1,{ { 121, 8},{  0, 0}},  0}, /* BETA1    */
00243       {13,  16, -2,  1.0L, 1,{ { 129, 8},{  0, 0}},  0}, /* BETA2    */
00244       {14,  16, -3,  1.0L, 1,{ { 137, 8},{  0, 0}},  0}, /* BETA3    */
00245       {15, -30,  0,  1.0L, 1,{ { 181,24},{211, 8}},  0}, /* A0       */
00246       {16, -50,  0,  1.0L, 1,{ { 151,24},{  0, 0}},  0}, /* A1       */
00247       {17,  12,  0,  1.0L, 0,{ { 219, 8},{  0, 0}},  0}, /* Tot      */
00248       {18,   0,  0,  1.0L, 0,{ { 227, 8},{  0, 0}},  0}, /* wnt      */
00249       {19,   0,  0,  1.0L, 1,{ { 241, 8},{  0, 0}},  0}, /* DELTATLS */
00250       {20,   0,  0,  1.0L, 0,{ { 249, 8},{  0, 0}},  0}, /* WN LSF   */
00251       {21,   0,  0,  1.0L, 0,{ { 257, 8},{  0, 0}},  0}, /* DN       */
00252       {22,   0,  0,  1.0L, 1,{ { 271, 8},{  0, 0}},  0}, /* DELTALSF */
00253          /* Pattern 9 */
00254       {0,  0,  0,  1.0L, 0, { {   1,  8}, { 0, 0}},  0}, /* Preamble */
00255       {1,  0,  0,  1.0L, 0, { {   9, 14}, { 0, 0}},  0}, /* Message  */
00256       {2,  0,  0,  6.0L, 0, { {  31, 17}, { 0, 0}},  0}, /* HOW      */
00257       {3,  0,  0,  1.0L, 0, { {  48,  2}, { 0, 0}},  0}, /* "alert"  */
00258       {4,  0,  0,  1.0L, 0, { {  50,  3}, { 0, 0}},  0}, /* SF ID    */
00259       {5,  0,  0,  1.0L, 0, { {  61,  2}, { 0, 0}},  0}, /* Dataflag */
00260       {6,  0,  0,  1.0L, 0, { {  63,  6}, { 0, 0}},  0}, /* Page ID  */
00261       {7,  0,  0,  1.0L, 0, { {  69,  4}, { 0, 0}},  0}, /* SV1 cnfig*/
00262       {8,  0,  0,  1.0L, 0, { {  73,  4}, { 0, 0}},  0}, /* SV2 cnfig*/
00263       {9,  0,  0,  1.0L, 0, { {  77,  4}, { 0, 0}},  0}, /* SV3 cnfig*/
00264       {10, 0,  0,  1.0L, 0, { {  81,  4}, { 0, 0}},  0}, /* SV4 cnfig*/
00265       {11, 0,  0,  1.0L, 0, { {  91,  4}, { 0, 0}},  0}, /* SV5 cnfig*/
00266       {12, 0,  0,  1.0L, 0, { {  95,  4}, { 0, 0}},  0}, /* SV6 cnfig*/
00267       {13, 0,  0,  1.0L, 0, { {  99,  4}, { 0, 0}},  0}, /* SV7 cnfig*/
00268       {14, 0,  0,  1.0L, 0, { { 103,  4}, { 0, 0}},  0}, /* SV8 cnfig*/
00269       {15, 0,  0,  1.0L, 0, { { 107,  4}, { 0, 0}},  0}, /* SV9 cnfig*/
00270       {16, 0,  0,  1.0L, 0, { { 111,  4}, { 0, 0}},  0}, /* SV10 cnfig*/
00271       {17, 0,  0,  1.0L, 0, { { 121,  4}, { 0, 0}},  0}, /* SV11 cnfig*/
00272       {18, 0,  0,  1.0L, 0, { { 125,  4}, { 0, 0}},  0}, /* SV12 cnfig*/
00273       {19, 0,  0,  1.0L, 0, { { 129,  4}, { 0, 0}},  0}, /* SV13 cnfig*/
00274       {20, 0,  0,  1.0L, 0, { { 133,  4}, { 0, 0}},  0}, /* SV14 cnfig*/
00275       {21, 0,  0,  1.0L, 0, { { 137,  4}, { 0, 0}},  0}, /* SV15 cnfig*/
00276       {22, 0,  0,  1.0L, 0, { { 141,  4}, { 0, 0}},  0}, /* SV16 cnfig*/
00277       {23, 0,  0,  1.0L, 0, { { 151,  4}, { 0, 0}},  0}, /* SV17 cnfig*/
00278       {24, 0,  0,  1.0L, 0, { { 155,  4}, { 0, 0}},  0}, /* SV18 cnfig*/
00279       {25, 0,  0,  1.0L, 0, { { 159,  4}, { 0, 0}},  0}, /* SV19 cnfig*/
00280       {26, 0,  0,  1.0L, 0, { { 163,  4}, { 0, 0}},  0}, /* SV20 cnfig*/
00281       {27, 0,  0,  1.0L, 0, { { 167,  4}, { 0, 0}},  0}, /* SV21 cnfig*/
00282       {28, 0,  0,  1.0L, 0, { { 171,  4}, { 0, 0}},  0}, /* SV22 cnfig*/
00283       {29, 0,  0,  1.0L, 0, { { 181,  4}, { 0, 0}},  0}, /* SV23 cnfig*/
00284       {30, 0,  0,  1.0L, 0, { { 185,  4}, { 0, 0}},  0}, /* SV24 cnfig*/
00285       {31, 0,  0,  1.0L, 0, { { 189,  4}, { 0, 0}},  0}, /* SV25 cnfig*/
00286       {32, 0,  0,  1.0L, 0, { { 193,  4}, { 0, 0}},  0}, /* SV26 cnfig*/
00287       {33, 0,  0,  1.0L, 0, { { 197,  4}, { 0, 0}},  0}, /* SV27 cnfig*/
00288       {34, 0,  0,  1.0L, 0, { { 201,  4}, { 0, 0}},  0}, /* SV28 cnfig*/
00289       {35, 0,  0,  1.0L, 0, { { 211,  4}, { 0, 0}},  0}, /* SV29 cnfig*/
00290       {36, 0,  0,  1.0L, 0, { { 215,  4}, { 0, 0}},  0}, /* SV30 cnfig*/
00291       {37, 0,  0,  1.0L, 0, { { 219,  4}, { 0, 0}},  0}, /* SV31 cnfig*/
00292       {38, 0,  0,  1.0L, 0, { { 223,  4}, { 0, 0}},  0}, /* SV32 cnfig*/
00293       {39, 0,  0,  1.0L, 0, { { 229,  6}, { 0, 0}},  0}, /* SV25 Hlth */
00294       {40, 0,  0,  1.0L, 0, { { 241,  6}, { 0, 0}},  0}, /* SV26 Hlth */
00295       {41, 0,  0,  1.0L, 0, { { 247,  6}, { 0, 0}},  0}, /* SV27 Hlth */
00296       {42, 0,  0,  1.0L, 0, { { 253,  6}, { 0, 0}},  0}, /* SV28 Hlth */
00297       {43, 0,  0,  1.0L, 0, { { 259,  6}, { 0, 0}},  0}, /* SV29 Hlth */
00298       {44, 0,  0,  1.0L, 0, { { 271,  6}, { 0, 0}},  0}, /* SV30 Hlth */
00299       {45, 0,  0,  1.0L, 0, { { 277,  6}, { 0, 0}},  0}, /* SV31 Hlth */
00300       {46, 0,  0,  1.0L, 0, { { 283,  6}, { 0, 0}},  0}, /* SV32 Hlth */
00301          /* Pattern 10 */
00302       {0,   0,  0,  1.0L, 0,{ {  1,   8}, {  0, 0}},  0}, /* Preamble */
00303       {1,   0,  0,  1.0L, 0,{ {  9,  14}, {  0, 0}},  0}, /* Message  */
00304       {2,   0,  0,  6.0L, 0,{ {  31, 17}, {  0, 0}},  0}, /* HOW      */
00305       {3,   0,  0,  1.0L, 0,{ {  48,  2}, {  0, 0}},  0}, /* "alert"  */
00306       {4,   0,  0,  1.0L, 0,{ {  50,  3}, {  0, 0}},  0}, /* SF ID    */
00307       {5,   0,  0,  1.0L, 0,{ {  61,  2}, {  0, 0}},  0}, /* Dataflag */
00308       {6,   0,  0,  1.0L, 0,{ {  63,  6}, {  0, 0}},  0}, /* Page ID  */
00309       {7,   0,  0,  1.0L, 0,{ {  69,  8}, {  0, 0}},  0}, /* ASCII    */
00310       {8,   0,  0,  1.0L, 0,{ {  77,  8}, {  0, 0}},  0}, /* ASCII    */
00311       {9,   0,  0,  1.0L, 0,{ {  91,  8}, {  0, 0}},  0}, /* ASCII    */
00312       {10,  0,  0,  1.0L, 0,{ {  99,  8}, {  0, 0}},  0}, /* ASCII    */
00313       {11,  0,  0,  1.0L, 0,{ { 107,  8}, {  0, 0}},  0}, /* ASCII    */
00314       {12,  0,  0,  1.0L, 0,{ { 121,  8}, {  0, 0}},  0}, /* ASCII    */
00315       {13,  0,  0,  1.0L, 0,{ { 129,  8}, {  0, 0}},  0}, /* ASCII    */
00316       {14,  0,  0,  1.0L, 0,{ { 137,  8}, {  0, 0}},  0}, /* ASCII    */
00317       {15,  0,  0,  1.0L, 0,{ { 151,  8}, {  0, 0}},  0}, /* ASCII    */
00318       {16,  0,  0,  1.0L, 0,{ { 159,  8}, {  0, 0}},  0}, /* ASCII    */
00319       {17,  0,  0,  1.0L, 0,{ { 167,  8}, {  0, 0}},  0}, /* ASCII    */
00320       {18,  0,  0,  1.0L, 0,{ { 181,  8}, {  0, 0}},  0}, /* ASCII    */
00321       {19,  0,  0,  1.0L, 0,{ { 189,  8}, {  0, 0}},  0}, /* ASCII    */
00322       {20,  0,  0,  1.0L, 0,{ { 197,  8}, {  0, 0}},  0}, /* ASCII    */
00323       {21,  0,  0,  1.0L, 0,{ { 211,  8}, {  0, 0}},  0}, /* ASCII    */
00324       {22,  0,  0,  1.0L, 0,{ { 219,  8}, {  0, 0}},  0}, /* ASCII    */
00325       {23,  0,  0,  1.0L, 0,{ { 227,  8}, {  0, 0}},  0}, /* ASCII    */
00326       {24,  0,  0,  1.0L, 0,{ { 241,  8}, {  0, 0}},  0}, /* ASCII    */
00327       {25,  0,  0,  1.0L, 0,{ { 249,  8}, {  0, 0}},  0}, /* ASCII    */
00328       {26,  0,  0,  1.0L, 0,{ { 257,  8}, {  0, 0}},  0}, /* ASCII    */
00329       {27,  0,  0,  1.0L, 0,{ { 271,  8}, {  0, 0}},  0}, /* ASCII    */
00330       {28,  0,  0,  1.0L, 0,{ { 279,  8}, {  0, 0}},  0}, /* ASCII    */
00331          /* Dummy pattern for constructor */
00332       {0,   0,  0,  1.0L, 0,{{  0,    0}, {  0, 0}},  0}
00333    };
00334 
00335 
00336    EngNav::EngNav()
00337       throw()
00338    {
00339       short i=0, n=0;
00340       static short initialized = 0;
00341 
00342       if (initialized)
00343          return;
00344 
00345          // Set up pointers within subframe format structure.
00346       for (i=1; i<=10; i++)
00347       {
00348          subframeList[i] = &formats[n];
00349          while (formats[n+1].outIndex != 0)
00350          {
00351             formats[n].nxtq = &formats[n+1];
00352             n++;
00353          }
00354          formats[n].nxtq = NULL;
00355          n++;
00356       }
00357 
00358          // Calculate powers of PI table.
00359       PItab[3] = 1.0L;
00360       for (i=0;i<=2;i++)
00361       {
00362          PItab[4+i] = PItab[3+i] * PI;
00363          PItab[2-i] = PItab[3-i] / PI;
00364       }
00365 
00366       initialized = 1;
00367    }
00368 
00369       // Retained for backward compatibility
00370    bool EngNav :: subframeConvert(const long input[10], 
00371                                   int gpsWeek,
00372                                   double output[60])
00373       throw()
00374    {
00375       uint32_t tinput[10];
00376       for (int n=0;n<10;++n) 
00377          tinput[n] = static_cast<uint32_t>( input[n] );
00378       short tgpsWeek = gpsWeek;
00379       return( subframeConvert( tinput, tgpsWeek, output ));
00380    }
00381    
00382    bool EngNav :: subframeConvert(const uint32_t input[10], 
00383                                   short gpsWeek,
00384                                   double output[60])
00385       throw()
00386    {
00387       short patId = -2, i = 2;
00388       struct DecodeQuant *p=NULL;
00389 
00390       for (i=0; i< 20; i++)
00391          output[i] = 0.0L;
00392 
00393          // Get subframe and page id and derive pattern number.
00394       if ((patId = getSubframePattern(input)) == 0)
00395          return false;
00396 
00397          // convert each quantity in the list
00398       p = subframeList[patId];
00399       while (p != NULL)
00400       {
00401          convertQuant(input, output, p);
00402          if(p->nxtq)
00403             p = p->nxtq;
00404          else
00405             p = NULL;
00406       }
00407 
00408          // Almanac does not contain a reference week
00409          // However we need to put one in the FIC version of the Almanac
00410       if (patId==4)
00411       {
00412          output[18]= static_cast<double>( gpsWeek );
00413       }
00414 
00415          // Modify the 8-bit week numbers to be the full week numbers
00416       if (patId == 8)
00417       {
00418          if (!convert8bit(gpsWeek, &output[18]))
00419             return false;
00420 
00421          if (!convert8bit(gpsWeek, &output[20]))
00422             return false;
00423       }
00424 
00425          // Change the 10 bit week number in subframe 1 to full weeks
00426       if (patId == 1)
00427       {
00428          short week10Bit = static_cast<uint32_t>( output[5] );
00429          output[5] = 
00430              static_cast<double>( convertXBit(gpsWeek, week10Bit, BITS10) );
00431       }
00432       return true;
00433    }
00434 
00435       // Retained for backward compatibility
00436    bool EngNav :: convert8bit(int gpsWeek, double *output)
00437       throw()
00438    {
00439       short tgpsWeek = static_cast<short>( gpsWeek );
00440       short toutput = static_cast<short> ( *output );
00441       short retArg = convertXBit( tgpsWeek, toutput, BITS8 );
00442       *output = static_cast<double>(retArg);
00443       
00444       return true;
00445    }
00446 
00447       // Retained for backward compatibility
00448    bool EngNav :: convert10bit(int gpsWeek, double *output)
00449       throw()
00450    {
00451       short tgpsWeek = static_cast<short>( gpsWeek );
00452       short toutput = static_cast<short> ( *output );
00453       short retArg = convertXBit( tgpsWeek, toutput, BITS10 );
00454       *output = static_cast<double>(retArg);
00455       return true;
00456    }
00457    
00458    static short LIMIT[] = { 127,  511 };
00459    static short RANGE[] = { 256, 1024 };
00460    
00461    short EngNav :: convertXBit(short fullGPSWeek, 
00462                                short incompleteGPSWeek,
00463                                BitConvertType type)
00464    {
00465       short extension = fullGPSWeek - (fullGPSWeek % RANGE[type]);
00466       short target = extension + incompleteGPSWeek;
00467       
00468       short diff = target - fullGPSWeek;
00469       if (diff>LIMIT[type]) 
00470          target -= RANGE[type];
00471       else if (diff< -LIMIT[type])
00472          target += RANGE[type];
00473 
00474       return( target );
00475    }
00476 
00477       // Retained for backward compatibility
00478    short EngNav :: getSubframePattern(const long input[10])
00479       throw()
00480    {
00481       uint32_t tinput[10];
00482       for (int n=0;n<10;++n) 
00483          tinput[n] = static_cast<uint32_t>( input[n] );
00484       return( getSubframePattern( tinput ));      
00485    }
00486    
00487    short EngNav :: getSubframePattern(const uint32_t input[10])
00488       throw()
00489    {
00490       short iret, svid;
00491       long  itemp;
00492 
00493       short patId[]
00494          = {   5,   6,  6,   6, 10,  8,   6,  7,   7,  7,   7,  7,   9 };
00495          /*SVid   51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63 */
00496 
00497          //  Get subframe id.  If 1-3 return as patId.
00498       
00499       itemp = input[1];         /* move HOW to temp storage         */
00500       itemp >>= 8;           /* shift so subframe id is in 3 lsb */
00501       itemp &= 0x00000007L;  /* and mask off msbs                */
00502       iret = static_cast<short>( itemp );
00503          /* Not a valid sf id */
00504       if ( iret < 1 || iret > 5 )
00505          return(0);
00506       if ( iret < 4 )
00507          return(iret);
00508 
00509 
00510          /*   If subframe 1-5, get page id and look up patId      */
00511       itemp = input[2];      /* move word 3 into temp storage    */
00512       itemp >>= 22;          /* shift so SV ID is in 6 lsbs      */
00513       itemp &= 0x0000003FL;  /* and mask off msbs                */
00514       svid  = static_cast<short>( itemp );
00515       if ( svid <= 32 )
00516          iret = 4;            /* PRN orbit data */
00517       else
00518          iret = patId[svid-51];  /* look up pat id for almanac overhead
00519                                     information */
00520       return iret;
00521    }
00522 
00523    uint32_t EngNav :: computeParity(uint32_t sfword,
00524                                     uint32_t psfword,
00525                                     bool knownUpright)
00526    {
00527          /*
00528            This function is somewhat table-driven.  There is one
00529            element in bmask for each of the six parity bits.  Each
00530            element is a bit mask with bits set corresponding to the
00531            bits which are to be exclusive-OR'd together to form the
00532            parity check bit.  The following bit maps define the bmask
00533            array.  They were drawn from table 20-XIV of ICD-GPS-200C
00534            (10 OCT 1993).
00535         
00536            Bit in navigation message
00537                  bit1                             bit 30
00538            bit    12 3456 789. 1234 5678 9.12 3456 789.
00539            ---    -------------------------------------
00540            D25    11 1011 0001 1111 0011 0100 1000 0000
00541            D26    01 1101 1000 1111 1001 1010 0100 0000
00542            D27    10 1110 1100 0111 1100 1101 0000 0000
00543            D28    01 0111 0110 0011 1110 0110 1000 0000
00544            D29    10 1011 1011 0001 1111 0011 0100 0000
00545            D30    00 1011 0111 1010 1000 1001 1100 0000
00546          */
00547       uint32_t bmask[6] = { 0x3B1F3480L, 0x1D8F9A40L, 0x2EC7CD00L,
00548                             0x1763E680L, 0x2BB1F340L, 0x0B7A89C0L };
00549    
00550       uint32_t D = 0;
00551       uint32_t d = sfword;
00552       uint32_t D29 = getd29(psfword);
00553       uint32_t D30 = getd30(psfword);
00554 
00555          // If D30 of the previous subframe was set, complement the word
00556          // to get the source data bits.  This will also complement the
00557          // parity, but we don't need the original parity to compute the
00558          // new.
00559       if (D30 && !knownUpright)
00560          d = ~d;
00561       D |= ((D29 + BinUtils::countBits(bmask[0] & d)) % 2) << 5;
00562       D |= ((D30 + BinUtils::countBits(bmask[1] & d)) % 2) << 4;
00563       D |= ((D29 + BinUtils::countBits(bmask[2] & d)) % 2) << 3;
00564       D |= ((D30 + BinUtils::countBits(bmask[3] & d)) % 2) << 2;
00565       D |= ((D30 + BinUtils::countBits(bmask[4] & d)) % 2) << 1;
00566       D |= ((D29 + BinUtils::countBits(bmask[5] & d)) % 2);
00567 
00568       return D;
00569    }
00570 
00571    uint32_t EngNav :: fixParity(uint32_t sfword,
00572                                 uint32_t psfword,
00573                                 bool nib)
00574    {
00575       uint32_t bmask[6] = { 0x3B1F3480L, 0x1D8F9A40L, 0x2EC7CD00L,
00576                                  0x1763E680L, 0x2BB1F340L, 0x0B7A89C0L };
00577 
00578       uint32_t D = 0;
00579       uint32_t d = sfword;
00580       uint32_t D29 = getd29(psfword);
00581       uint32_t D30 = getd30(psfword);
00582 
00583       if (nib)
00584       {
00585             // make sure the non-information bits are zero to start with.
00586          d &= 0xffffff00;
00587          if ((D30 + BinUtils::countBits(bmask[4] & d)) % 2)
00588             d |= 0x00000040;
00589          if ((D29 + BinUtils::countBits(bmask[5] & d)) % 2)
00590             d |= 0x00000080;
00591       }
00592 
00593       D = computeParity(d, psfword);
00594 
00595       return D | d;
00596    }
00597 
00599    bool EngNav :: subframeParity(const long input[10])
00600    {
00601       uint32_t temp[10];
00602       for (int n=0;n<10;++n) temp[n] = input[n];
00603       return(checkParity( temp ));
00604    }
00605 
00606 
00607    bool EngNav :: checkParity(const std::vector<uint32_t>& sf, bool knownUpright)
00608    {
00609       return (((sf[0] & 0x0000003f) == computeParity(sf[0],     0, knownUpright)) &&
00610               ((sf[1] & 0x0000003f) == computeParity(sf[1], sf[0], knownUpright)) &&
00611               ((sf[2] & 0x0000003f) == computeParity(sf[2], sf[1], knownUpright)) &&
00612               ((sf[3] & 0x0000003f) == computeParity(sf[3], sf[2], knownUpright)) &&
00613               ((sf[4] & 0x0000003f) == computeParity(sf[4], sf[3], knownUpright)) &&
00614               ((sf[5] & 0x0000003f) == computeParity(sf[5], sf[4], knownUpright)) &&
00615               ((sf[6] & 0x0000003f) == computeParity(sf[6], sf[5], knownUpright)) &&
00616               ((sf[7] & 0x0000003f) == computeParity(sf[7], sf[6], knownUpright)) &&
00617               ((sf[8] & 0x0000003f) == computeParity(sf[8], sf[7], knownUpright)) &&
00618               ((sf[9] & 0x0000003f) == computeParity(sf[9], sf[8], knownUpright)));
00619    }
00620 
00621    bool EngNav :: checkParity(const uint32_t sf[10], bool knownUpright)
00622    {
00623       std::vector<uint32_t> temp(10);
00624       for (size_t n=0; n<10; ++n)
00625          temp[n] = sf[n];
00626       return checkParity(temp, knownUpright);
00627    }
00628 
00629    void EngNav :: convertQuant(const uint32_t input[10], 
00630                                double output[60],
00631                                DecodeQuant *p)
00632       throw()
00633    {
00634       double dval;
00635       short i, n, bit1, nword, nbit, lsb;
00636       union equ
00637       {
00638          uint32_t u;
00639          int32_t s;
00640       } temp;
00641       uint32_t *b;
00642       uint32_t mask;
00643 
00644          // Convert starting bit number to word/bit pair
00645       temp.u = 0x0L;
00646       for (n=0; n<=1; n++)
00647       {
00648          if (p->fmt[n].startBit == 0)
00649             break;
00650          bit1 = p->fmt[n].startBit;
00651          nword = (bit1-1) / 30;
00652          nbit  = (bit1 % 30) + 1;
00653 
00654          b = const_cast<uint32_t *>( input ) + nword;
00655          for (i=0;i<p->fmt[n].numBits;i++)
00656          {
00657             temp.u <<= 1;
00658             mask = 0x80000000L >> nbit++;
00659             if (*b & mask)
00660                temp.u++;
00661             if (nbit>=32)
00662             {
00663                b++;
00664                nbit = 0;
00665             }
00666          }
00667       }
00668 
00669          // Convert to double and scale
00670       if (p->signq)
00671       {
00672          nbit = 32 - (p->fmt[0].numBits + p->fmt[1].numBits);
00673          temp.u <<= nbit; // Move sign bit to msb
00674          temp.s >>= nbit; // Move lsb back to right spot with sign extend
00675          dval = temp.s;
00676       }
00677       else
00678       {
00679          dval = temp.u; // msb = 0
00680       }
00681       dval = dval * p->scale;             // Scale by scalar
00682       dval = dval * PItab[ p->powPI+3 ];  // Scale by power of PI
00683       dval = LDEXP(dval,p->pow2);         // Scale by power of 2
00684       output[p->outIndex] = dval;         // Store result in output array
00685    }
00686 
00687 } // namespace
Generated on Tue Jan 6 03:31:18 2009 for GPS ToolKit Software Library by  doxygen 1.3.9.1