Changeset 65 in ntrip for trunk/BNC/RTCM3/rtcm3.cpp


Ignore:
Timestamp:
Aug 24, 2006, 3:58:20 PM (18 years ago)
Author:
mervart
Message:

* empty log message *

Location:
trunk/BNC
Files:
2 edited

Legend:

Unmodified
Added
Removed

  • trunk/BNC

    • Property svn:ignore

      •  

        old new  
        22Makefile
        33bnc
         4anlagen
        45moc_bnccaster.cpp
        56moc_bncgetthread.cpp

  • trunk/BNC/RTCM3/rtcm3.cpp

    r53 r65  
    1 /*
    2   Converter for RTCM3 data to RINEX.
    3   $Id: rtcm3.cpp,v 1.1 2006/08/20 13:35:39 mervart Exp $
    41
    5   Program written bei
    6   Dirk Stoecker
    7   Euronik GmbH
    8   http://www.euronik.eu/
     2/* -------------------------------------------------------------------------
     3 * Bernese NTRIP Client
     4 * -------------------------------------------------------------------------
     5 *
     6 * Class:      rtcm3
     7 *
     8 * Purpose:    RTCM3 Decoder
     9 *
     10 * Author:     L. Mervart
     11 *
     12 * Created:    24-Aug-2006
     13 *
     14 * Changes:   
     15 *
     16 * -----------------------------------------------------------------------*/
    917
    10   for
     18#include "rtcm3.h"
    1119
    12   Federal Agency for Cartography and Geodesy (BKG)
    13   Richard-Strauss-Allee 11
    14   D-60598 Frankfurt
    15   http://igs.bkg.bund.de/index_ntrip.htm
     20using namespace std;
    1621
    17   Contact Dirk Stoecker [stoecker@euronik.eu] or [euref-ip@bkg.bund.de]
    18   with your comments, suggestions, improvements, patches.
    19 
    20   Copyright (C) 2005-2006 by German Federal Agency for Cartography and Geodesy
    21 
    22   This program is free software; you can redistribute it and/or modify
    23   it under the terms of the GNU General Public License as published by
    24   the Free Software Foundation; either version 2 of the License, or
    25   (at your option) any later version.
    26 
    27   This program is distributed in the hope that it will be useful,
    28   but WITHOUT ANY WARRANTY; without even the implied warranty of
    29   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    30   GNU General Public License for more details.
    31 
    32   You should have received a copy of the GNU General Public License
    33   along with this program; if not, write to the Free Software
    34   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
    35   or read http://www.gnu.org/licenses/gpl.txt
    36 */
    37 
    38 #include <ctype.h>
    39 #include <errno.h>
    40 #include <getopt.h>
    41 #include <math.h>
    42 #include <netdb.h>
    43 #include <netinet/in.h>
    44 #include <signal.h>
    45 #include <stdint.h>
    46 #include <stdio.h>
    47 #include <stdlib.h>
    48 #include <string.h>
    49 #include <sys/socket.h>
    50 #include <sys/types.h>
    51 #include <time.h>
    52 #include <unistd.h>
    53 
    54 /* The string, which is send as agent in HTTP request */
    55 #define AGENTSTRING "NTRIP NtripRTCM3ToRINEX"
    56 
    57 #define MAXDATASIZE 1000 /* max number of bytes we can get at once */
    58 
    59 /* CVS revision and version */
    60 static char revisionstr[] = "$Revision: 1.1 $";
    61 static char datestr[]     = "$Date: 2006/08/20 13:35:39 $";
    62 static int stop = 0;
    63 
    64 /* unimportant, only for approx. time needed */
    65 #define LEAPSECONDS 14
    66 
    67 struct converttimeinfo {
    68   int second;    /* seconds of GPS time [0..59] */
    69   int minute;    /* minutes of GPS time [0..59] */
    70   int hour;      /* hour of GPS time [0..24] */
    71   int day;       /* day of GPS time [1..28..30(31)*/
    72   int month;     /* month of GPS time [1..12]*/
    73   int year;      /* year of GPS time [1980..] */
    74 };
    75 
    76 #define PRN_GPS_START             1
    77 #define PRN_GPS_END               32
    78 #define PRN_GLONASS_START         38
    79 #define PRN_GLONASS_END           61
    80 
    81 #define GNSSENTRY_C1DATA     0
    82 #define GNSSENTRY_C2DATA     1
    83 #define GNSSENTRY_P1DATA     2
    84 #define GNSSENTRY_P2DATA     3
    85 #define GNSSENTRY_L1CDATA    4
    86 #define GNSSENTRY_L1PDATA    5
    87 #define GNSSENTRY_L2CDATA    6
    88 #define GNSSENTRY_L2PDATA    7
    89 #define GNSSENTRY_D1CDATA    8
    90 #define GNSSENTRY_D1PDATA    9
    91 #define GNSSENTRY_D2CDATA    10
    92 #define GNSSENTRY_D2PDATA    11
    93 #define GNSSENTRY_S1CDATA    12
    94 #define GNSSENTRY_S1PDATA    13
    95 #define GNSSENTRY_S2CDATA    14
    96 #define GNSSENTRY_S2PDATA    15
    97 #define GNSSENTRY_NUMBER     16 /* number of types!!! */
    98 
    99 /* Data flags. These flags are used in the dataflags field of gpsdata structure
    100    and are used the determine, which data fields are filled with valid data. */
    101 #define GNSSDF_C1DATA         (1<<GNSSENTRY_C1DATA)
    102 #define GNSSDF_C2DATA         (1<<GNSSENTRY_C2DATA)
    103 #define GNSSDF_P1DATA         (1<<GNSSENTRY_P1DATA)
    104 #define GNSSDF_P2DATA         (1<<GNSSENTRY_P2DATA)
    105 #define GNSSDF_L1CDATA        (1<<GNSSENTRY_L1CDATA)
    106 #define GNSSDF_L1PDATA        (1<<GNSSENTRY_L1PDATA)
    107 #define GNSSDF_L2CDATA        (1<<GNSSENTRY_L2CDATA)
    108 #define GNSSDF_L2PDATA        (1<<GNSSENTRY_L2PDATA)
    109 #define GNSSDF_D1CDATA        (1<<GNSSENTRY_D1CDATA)
    110 #define GNSSDF_D1PDATA        (1<<GNSSENTRY_D1PDATA)
    111 #define GNSSDF_D2CDATA        (1<<GNSSENTRY_D2CDATA)
    112 #define GNSSDF_D2PDATA        (1<<GNSSENTRY_D2PDATA)
    113 #define GNSSDF_S1CDATA        (1<<GNSSENTRY_S1CDATA)
    114 #define GNSSDF_S1PDATA        (1<<GNSSENTRY_S1PDATA)
    115 #define GNSSDF_S2CDATA        (1<<GNSSENTRY_S2CDATA)
    116 #define GNSSDF_S2PDATA        (1<<GNSSENTRY_S2PDATA)
    117 
    118 #define RINEXENTRY_C1DATA     0
    119 #define RINEXENTRY_C2DATA     1
    120 #define RINEXENTRY_P1DATA     2
    121 #define RINEXENTRY_P2DATA     3
    122 #define RINEXENTRY_L1DATA     4
    123 #define RINEXENTRY_L2DATA     5
    124 #define RINEXENTRY_D1DATA     6
    125 #define RINEXENTRY_D2DATA     7
    126 #define RINEXENTRY_S1DATA     8
    127 #define RINEXENTRY_S2DATA     9
    128 #define RINEXENTRY_NUMBER     10
    129 
    130 /* Additional flags for the data field, which tell us more. */
    131 #define GNSSDF_LOCKLOSSL1     (1<<29)  /* lost lock on L1 */
    132 #define GNSSDF_LOCKLOSSL2     (1<<30)  /* lost lock on L2 */
    133 
    134 struct gnssdata {
    135   int    flags;              /* GPSF_xxx */
    136   int    week;               /* week number of GPS date */
    137   int    numsats;
    138   double timeofweek;         /* milliseconds in GPS week */
    139   double measdata[24][GNSSENTRY_NUMBER];  /* data fields */
    140   int    dataflags[24];      /* GPSDF_xxx */
    141   int    satellites[24];     /* SV - IDs */
    142   int    snrL1[24];          /* Important: all the 5 SV-specific fields must */
    143   int    snrL2[24];          /* have the same SV-order */
    144 };
    145 
    146 struct RTCM3ParserData {
    147   unsigned char Message[2048]; /* input-buffer */
    148   int    MessageSize;   /* current buffer size */
    149   int    NeedBytes;     /* bytes wanted for next run */
    150   int    SkipBytes;     /* bytes to skip in next round */
    151   int    GPSWeek;
    152   int    GPSTOW;        /* in seconds */
    153   struct gnssdata Data;
    154   int    size;
    155   int    lastlockl1[64];
    156   int    lastlockl2[64];
    157   int    datapos[RINEXENTRY_NUMBER];
    158   int    dataflag[RINEXENTRY_NUMBER];
    159   int    numdatatypes;
    160   int    validwarning;
    161   int    init;
    162   const char * headerfile;
    163 };
    164 
    165 struct Args
    166 {
    167   const char *server;
    168   int         port;
    169   const char *user;
    170   const char *password;
    171   const char *data;
    172   const char *headerfile;
    173 };
    174 
    175 /* option parsing */
    176 #ifdef NO_LONG_OPTS
    177 #define LONG_OPT(a)
    178 #else
    179 #define LONG_OPT(a) a
    180 static struct option opts[] = {
    181 { "data",       required_argument, 0, 'd'},
    182 { "server",     required_argument, 0, 's'},
    183 { "password",   required_argument, 0, 'p'},
    184 { "port",       required_argument, 0, 'r'},
    185 { "header",     required_argument, 0, 'f'},
    186 { "user",       required_argument, 0, 'u'},
    187 { "help",       no_argument,       0, 'h'},
    188 {0,0,0,0}};
    189 #endif
    190 #define ARGOPT "d:hp:r:s:u:f:"
    191 
    192 static int getargs(int argc, char **argv, struct Args *args)
    193 {
    194   int res = 1;
    195   int getoptr;
    196   int help = 0;
    197   char *t;
    198 
    199   args->server = "www.euref-ip.net";
    200   args->port = 80;
    201   args->user = "";
    202   args->password = "";
    203   args->data = 0;
    204   args->headerfile = 0;
    205   help = 0;
    206 
    207   do
    208   {
    209 #ifdef NO_LONG_OPTS
    210     switch((getoptr = getopt(argc, argv, ARGOPT)))
    211 #else
    212     switch((getoptr = getopt_long(argc, argv, ARGOPT, opts, 0)))
    213 #endif
    214     {
    215     case 's': args->server = optarg; break;
    216     case 'u': args->user = optarg; break;
    217     case 'p': args->password = optarg; break;
    218     case 'd': args->data = optarg; break;
    219     case 'f': args->headerfile = optarg; break;
    220     case 'h': help=1; break;
    221     case 'r':
    222       args->port = strtoul(optarg, &t, 10);
    223       if((t && *t) || args->port < 1 || args->port > 65535)
    224         res = 0;
    225       break;
    226     case -1: break;
    227     }
    228   } while(getoptr != -1 || !res);
    229 
    230   datestr[0] = datestr[7];
    231   datestr[1] = datestr[8];
    232   datestr[2] = datestr[9];
    233   datestr[3] = datestr[10];
    234   datestr[5] = datestr[12];
    235   datestr[6] = datestr[13];
    236   datestr[8] = datestr[15];
    237   datestr[9] = datestr[16];
    238   datestr[4] = datestr[7] = '-';
    239   datestr[10] = 0;
    240 
    241   if(!res || help)
    242   {
    243     fprintf(stderr, "Version %s (%s) GPL\nUsage: %s -s server -u user ...\n"
    244     " -d " LONG_OPT("--data       ") "the requested data set\n"
    245     " -f " LONG_OPT("--headerfile ") "file for RINEX header information\n"
    246     " -s " LONG_OPT("--server     ") "the server name or address\n"
    247     " -p " LONG_OPT("--password   ") "the login password\n"
    248     " -r " LONG_OPT("--port       ") "the server port number (default 80)\n"
    249     " -u " LONG_OPT("--user       ") "the user name\n"
    250     , revisionstr, datestr, argv[0]);
    251     exit(1);
    252   }
    253   return res;
     22// Constructor
     23////////////////////////////////////////////////////////////////////////////
     24rtcm3::rtcm3() : GPSDecoder() {
     25  memset(&_Parser, 0, sizeof(_Parser));
     26  time_t tim;
     27  tim = time(0) - ((10*365+2+5)*24*60*60 + LEAPSECONDS);
     28  _Parser.GPSWeek = tim/(7*24*60*60);
     29  _Parser.GPSTOW = tim%(7*24*60*60);
    25430}
    25531
    256 static const char encodingTable [64] = {
    257   'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P',
    258   'Q','R','S','T','U','V','W','X','Y','Z','a','b','c','d','e','f',
    259   'g','h','i','j','k','l','m','n','o','p','q','r','s','t','u','v',
    260   'w','x','y','z','0','1','2','3','4','5','6','7','8','9','+','/'
    261 };
    262 
    263 /* does not buffer overrun, but breaks directly after an error */
    264 /* returns the number of required bytes */
    265 static int encode(char *buf, int size, const char *user, const char *pwd)
    266 {
    267   unsigned char inbuf[3];
    268   char *out = buf;
    269   int i, sep = 0, fill = 0, bytes = 0;
    270 
    271   while(*user || *pwd)
    272   {
    273     i = 0;
    274     while(i < 3 && *user) inbuf[i++] = *(user++);
    275     if(i < 3 && !sep)    {inbuf[i++] = ':'; ++sep; }
    276     while(i < 3 && *pwd)  inbuf[i++] = *(pwd++);
    277     while(i < 3)         {inbuf[i++] = 0; ++fill; }
    278     if(out-buf < size-1)
    279       *(out++) = encodingTable[(inbuf [0] & 0xFC) >> 2];
    280     if(out-buf < size-1)
    281       *(out++) = encodingTable[((inbuf [0] & 0x03) << 4)
    282                | ((inbuf [1] & 0xF0) >> 4)];
    283     if(out-buf < size-1)
    284     {
    285       if(fill == 2)
    286         *(out++) = '=';
    287       else
    288         *(out++) = encodingTable[((inbuf [1] & 0x0F) << 2)
    289                  | ((inbuf [2] & 0xC0) >> 6)];
    290     }
    291     if(out-buf < size-1)
    292     {
    293       if(fill >= 1)
    294         *(out++) = '=';
    295       else
    296         *(out++) = encodingTable[inbuf [2] & 0x3F];
    297     }
    298     bytes += 4;
    299   }
    300   if(out-buf < size)
    301     *out = 0;
    302   return bytes;
     32// Destructor
     33////////////////////////////////////////////////////////////////////////////
     34rtcm3::~rtcm3() {
    30335}
    30436
    305 static uint32_t CRC24(long size, const unsigned char *buf)
    306 {
    307   uint32_t crc = 0;
    308   int i;
    309 
    310   while(size--)
    311   {
    312     crc ^= (*buf++) << (16);
    313     for(i = 0; i < 8; i++)
    314     {
    315       crc <<= 1;
    316       if(crc & 0x1000000)
    317         crc ^= 0x01864cfb;
    318     }
    319   }
    320   return crc;
    321 }
    322 
    323 static int GetMessage(struct RTCM3ParserData *handle)
    324 {
    325   unsigned char *m, *e;
    326   int i;
    327 
    328   m = handle->Message+handle->SkipBytes;
    329   e = handle->Message+handle->MessageSize;
    330   handle->NeedBytes = handle->SkipBytes = 0;
    331   while(e-m >= 3)
    332   {
    333     if(m[0] == 0xD3)
    334     {
    335       handle->size = ((m[1]&3)<<8)|m[2];
    336       if(e-m >= handle->size+6)
    337       {
    338         if((uint32_t)((m[3+handle->size]<<16)|(m[3+handle->size+1]<<8)
    339         |(m[3+handle->size+2])) == CRC24(handle->size+3, m))
    340         {
    341           handle->SkipBytes = handle->size;
    342           break;
    343         }
    344         else
    345           ++m;
    346       }
    347       else
    348       {
    349         handle->NeedBytes = handle->size+6;
    350         break;
    351       }
    352     }
    353     else
    354       ++m;
    355   }
    356   if(e-m < 3)
    357     handle->NeedBytes = 3;
    358  
    359   /* copy buffer to front */
    360   i = m - handle->Message;
    361   if(i && m < e)
    362     memmove(handle->Message, m, handle->MessageSize-i);
    363   handle->MessageSize -= i;
    364 
    365   return !handle->NeedBytes;
    366 }
    367 
    368 #define LOADBITS(a) \
    369 { \
    370   while((a) > numbits) \
    371   { \
    372     if(!size--) break; \
    373     bitfield = (bitfield<<8)|*(data++); \
    374     numbits += 8; \
    375   } \
    376 }
    377 
    378 /* extract bits from data stream
    379    b = variable to store result, a = number of bits */
    380 #define GETBITS(b, a) \
    381 { \
    382   LOADBITS(a) \
    383   b = (bitfield<<(64-numbits))>>(64-(a)); \
    384   numbits -= (a); \
    385 }
    386 
    387 /* extract bits from data stream
    388    b = variable to store result, a = number of bits */
    389 #define GETBITSSIGN(b, a) \
    390 { \
    391   LOADBITS(a) \
    392   b = ((int64_t)(bitfield<<(64-numbits)))>>(64-(a)); \
    393   numbits -= (a); \
    394 }
    395 
    396 #define SKIPBITS(b) { LOADBITS(b) numbits -= (b); }
    397 
    398 static int RTCM3Parser(struct RTCM3ParserData *handle)
    399 {
    400   int ret=0;
    401 
    402   while(!ret && GetMessage(handle))
    403   {
    404     /* using 64 bit integer types, as it is much easier than handling
    405     the long datatypes in 32 bit */
    406     uint64_t numbits = 0, bitfield = 0;
    407     int size = handle->size, type;
    408     unsigned char *data = handle->Message+3;
    409 
    410     GETBITS(type,12)
    411     switch(type)
    412     {
    413     case 1001: case 1002: case 1003: case 1004:
    414       if(handle->GPSWeek)
    415       {
    416         int lastlockl1[64];
    417         int lastlockl2[64];
    418         struct gnssdata *gnss;
    419         int i, num, wasamb=0;
    420 
    421         for(i = 0; i < 64; ++i)
    422           lastlockl1[i] = lastlockl2[i] = 0;
    423 
    424         gnss = &handle->Data;
    425         memset(gnss, 0, sizeof(*gnss));
    426 
    427         SKIPBITS(12) /* id */
    428         GETBITS(i,30)
    429         if(i/1000 < (int)handle->GPSTOW - 86400)
    430           ++handle->GPSWeek;
    431         handle->GPSTOW = i/1000;
    432         gnss->timeofweek = i;
    433         gnss->week = handle->GPSWeek;
    434 
    435         SKIPBITS(1) /* sync */
    436         GETBITS(i,5)
    437         gnss->numsats = i;
    438         SKIPBITS(4) /* smind, smint */
    439 
    440         for(num = 0; num < gnss->numsats; ++num)
    441         {
    442           int sv, code, l1range, c,l,s,ce,le,se,amb=0;
    443 
    444           GETBITS(sv, 6);
    445           gnss->satellites[num] = (sv < 40 ? sv : sv+80);
    446           /* L1 */
    447           GETBITS(code, 1);
    448           if(code)
    449           {
    450             c = GNSSDF_P1DATA;  ce = GNSSENTRY_P1DATA;
    451             l = GNSSDF_L1PDATA; le = GNSSENTRY_L1PDATA;
    452             s = GNSSDF_S1PDATA; se = GNSSENTRY_S1PDATA;
    453           }
    454           else
    455           {
    456             c = GNSSDF_C1DATA;  ce = GNSSENTRY_C1DATA;
    457             l = GNSSDF_L1CDATA; le = GNSSENTRY_L1CDATA;
    458             s = GNSSDF_S1CDATA; se = GNSSENTRY_S1CDATA;
    459           }
    460           GETBITS(l1range, 24);
    461           if(l1range != 0x80000)
    462           {
    463             gnss->dataflags[num] |= c;
    464             gnss->measdata[num][ce] = l1range*0.02;
    465           }
    466           GETBITSSIGN(i, 20);
    467           if(i != 0x80000)
    468           {
    469             gnss->dataflags[num] |= l;
    470             gnss->measdata[num][le] = l1range*0.02+i*0.0005;
    471           }
    472           GETBITS(i, 7);
    473           lastlockl1[sv] = i;
    474           if(handle->lastlockl1[sv] > i)
    475             gnss->dataflags[num] |= GNSSDF_LOCKLOSSL1;
    476           if(type == 1002 || type == 1004)
    477           {
    478             GETBITS(amb,8);
    479             if(amb && (gnss->dataflags[num] & c))
    480             {
    481               gnss->measdata[num][ce] += amb*299792.458;
    482               gnss->measdata[num][le] += amb*299792.458;
    483               ++wasamb;
    484             }
    485             GETBITS(i, 8);
    486             if(i)
    487             {
    488               gnss->dataflags[num] |= s;
    489               gnss->measdata[num][se] = i*0.25;
    490               i /= 4*4;
    491               if(i > 9) i = 9;
    492               else if(i < 1) i = 1;
    493               gnss->snrL1[num] = i;
    494             }
    495           }
    496           if(type == 1003 || type == 1004)
    497           {
    498             /* L2 */
    499             GETBITS(code,2);
    500             if(code)
    501             {
    502               c = GNSSDF_P2DATA;  ce = GNSSENTRY_P2DATA;
    503               l = GNSSDF_L2PDATA; le = GNSSENTRY_L2PDATA;
    504               s = GNSSDF_S2PDATA; se = GNSSENTRY_S2PDATA;
    505             }
    506             else
    507             {
    508               c = GNSSDF_C2DATA;  ce = GNSSENTRY_C2DATA;
    509               l = GNSSDF_L2CDATA; le = GNSSENTRY_L2CDATA;
    510               s = GNSSDF_S2CDATA; se = GNSSENTRY_S2CDATA;
    511             }
    512             GETBITSSIGN(i,14);
    513             if(i != 0x2000)
    514             {
    515               gnss->dataflags[num] |= c;
    516               gnss->measdata[num][ce] = l1range*0.02+i*0.02
    517               +amb*299792.458;
    518             }
    519             GETBITSSIGN(i,20);
    520             if(i != 0x80000)
    521             {
    522               gnss->dataflags[num] |= l;
    523               gnss->measdata[num][le] = l1range*0.02+i*0.0005
    524               +amb*299792.458;
    525             }
    526             GETBITS(i,7);
    527             lastlockl2[sv] = i;
    528             if(handle->lastlockl2[sv] > i)
    529               gnss->dataflags[num] |= GNSSDF_LOCKLOSSL2;
    530             if(type == 1004)
    531             {
    532               GETBITS(i, 8);
    533               if(i)
    534               {
    535                 gnss->dataflags[num] |= s;
    536                 gnss->measdata[num][se] = i*0.25;
    537                 i /= 4*4;
    538                 if(i > 9) i = 9;
    539                 else if(i < 1) i = 1;
    540                 gnss->snrL2[num] = i;
    541               }
    542             }
    543           }
    544         }
    545         for(i = 0; i < 64; ++i)
    546         {
    547           handle->lastlockl1[i] = lastlockl1[i];
    548           handle->lastlockl2[i] = lastlockl2[i];
    549         }
    550         if(wasamb) /* not RINEX compatible without */
    551           ret = 1;
    552         else
    553           ret = 2;
    554       }
    555       break;
    556     }
    557   }
    558   return ret;
    559 }
    560 
    561 static int longyear(int year, int month)
    562 {
    563   if(!(year % 4) && (!(year % 400) || (year % 100)))
    564   {
    565     if(!month || month == 2)
    566       return 1;
    567   }
    568   return 0;
    569 }
    570 
    571 static void converttime(struct converttimeinfo *c, int week, int tow)
    572 {
    573   /* static variables */
    574   static const int months[13] = {0,31,28,31,30,31,30,31,31,30,31,30,31};
    575 
    576   int i, k, doy, j; /* temporary variables */
    577   j = week*(7*24*60*60) + tow + 5*24*60*60;
    578   for(i = 1980; j >= (k = (365+longyear(i,0))*24*60*60); ++i)
    579     j -= k;
    580   c->year = i;
    581   doy = 1+ (j / (24*60*60));
    582   j %= (24*60*60);
    583   c->hour = j / (60*60);
    584   j %= (60*60);
    585   c->minute = j / 60;
    586   c->second = j % 60;
    587   j = 0;
    588   for(i = 1; j + (k = months[i] + longyear(c->year,i)) < doy; ++i)
    589     j += k;
    590   c->month = i;
    591   c->day = doy - j;
    592 }
    593 
    594 struct Header
    595 {
    596   const char *version;
    597   const char *pgm;
    598   const char *marker;
    599   const char *observer;
    600   const char *receiver;
    601   const char *antenna;
    602   const char *position;
    603   const char *antennaposition;
    604   const char *wavelength;
    605   const char *typesofobs; /* should not be modified outside */
    606   const char *timeoffirstobs; /* should not be modified outside */
    607 };
    608 
    609 #define MAXHEADERLINES 50
    610 #define MAXHEADERBUFFERSIZE 4096
    611 struct HeaderData
    612 {
    613   union
    614   {
    615     struct Header named;
    616     const char *unnamed[MAXHEADERLINES];
    617   } data;
    618   int  numheaders;
    619 };
    620 
    621 static void HandleHeader(struct RTCM3ParserData *Parser)
    622 {
    623   struct HeaderData hdata;
    624   char thebuffer[MAXHEADERBUFFERSIZE];
    625   char *buffer = thebuffer;
    626   int buffersize = sizeof(thebuffer);
    627   int i;
    628 
    629   hdata.data.named.version =
    630   "     2.11           OBSERVATION DATA    M (Mixed)"
    631   "           RINEX VERSION / TYPE";
    632 
    633   {
    634     const char *str;
    635     time_t t;
    636     struct tm * t2;
    637 
    638     str = getenv("USER");
    639     if(!str) str = "";
    640     t = time(&t);
    641     t2 = gmtime(&t);
    642     hdata.data.named.pgm = buffer;
    643     i = 1+snprintf(buffer, buffersize,
    644     "RTCM3TORINEX %-7.7s%-20.20s%04d-%02d-%02d %02d:%02d    "
    645     "PGM / RUN BY / DATE",
    646     revisionstr, str, 1900+t2->tm_year, t2->tm_mon+1, t2->tm_mday, t2->tm_hour,
    647     t2->tm_min);
    648     buffer += i; buffersize -= i;
    649 
    650     hdata.data.named.observer = buffer;
    651     i = 1+snprintf(buffer, buffersize,
    652     "%-20.20s                                        "
    653     "OBSERVER / AGENCY", str);
    654     buffer += i; buffersize -= i;
    655   }
    656 
    657   hdata.data.named.marker =
    658   "RTCM3TORINEX                                                "
    659   "MARKER NAME";
    660 
    661   hdata.data.named.receiver =
    662   "                                                            "
    663   "REC # / TYPE / VERS";
    664 
    665   hdata.data.named.antenna =
    666   "                                                            "
    667   "ANT # / TYPE";
    668 
    669   hdata.data.named.position =
    670   "         .0000         .0000         .0000                  "
    671   "APPROX POSITION XYZ";
    672 
    673   hdata.data.named.antennaposition =
    674   "         .0000         .0000         .0000                  "
    675   "ANTENNA: DELTA H/E/N";
    676  
    677   hdata.data.named.wavelength =
    678   "     1     1                                                "
    679   "WAVELENGTH FACT L1/2";
    680 
    681   {
    682 #define CHECKFLAGS(a, b) \
    683     if(flags & GNSSDF_##a##DATA \
    684     && !data[RINEXENTRY_##b##DATA]) \
    685     { \
    686       Parser->dataflag[Parser->numdatatypes] = GNSSDF_##a##DATA; \
    687       Parser->datapos[Parser->numdatatypes++] = data[RINEXENTRY_##b##DATA] \
    688       = GNSSENTRY_##a##DATA; \
    689       snprintf(tbuffer+tbufferpos, sizeof(tbuffer)-tbufferpos, "    "#b); \
    690       tbufferpos += 6; \
    691     }
    692 
    693     int flags = 0;
    694     int data[RINEXENTRY_NUMBER];
    695     char tbuffer[6*RINEXENTRY_NUMBER+1];
    696     int tbufferpos = 0;
    697     for(i = 0; i < RINEXENTRY_NUMBER; ++i)
    698       data[i] = 0;
    699     for(i = 0; i < Parser->Data.numsats; ++i)
    700       flags |= Parser->Data.dataflags[i];
    701 
    702     CHECKFLAGS(C1,C1)
    703     CHECKFLAGS(C2,C2)
    704     CHECKFLAGS(P1,P1)
    705     CHECKFLAGS(P2,P2)
    706     CHECKFLAGS(L1C,L1)
    707     CHECKFLAGS(L1P,L1)
    708     CHECKFLAGS(L2C,L2)
    709     CHECKFLAGS(L2P,L2)
    710     CHECKFLAGS(D1C,D1)
    711     CHECKFLAGS(D1P,D1)
    712     CHECKFLAGS(D2C,D2)
    713     CHECKFLAGS(D2P,D2)
    714     CHECKFLAGS(S1C,S1)
    715     CHECKFLAGS(S1P,S1)
    716     CHECKFLAGS(S2C,S2)
    717     CHECKFLAGS(S2P,S2)
    718 
    719     hdata.data.named.typesofobs = buffer;
    720     i = 1+snprintf(buffer, buffersize,
    721     "%6i%-54.54s# / TYPES OF OBSERV", Parser->numdatatypes, tbuffer);
    722     if(Parser->numdatatypes>9)
    723     {
    724       i += snprintf(buffer+i-1, buffersize,
    725       "\n      %-54.54s# / TYPES OF OBSERV", tbuffer+9*6);
    726     }
    727     buffer += i; buffersize -= i;
    728   }
    729 
    730   {
    731     struct converttimeinfo cti;
    732     converttime(&cti, Parser->Data.week,
    733                 int(floor(Parser->Data.timeofweek/1000.0)));
    734     hdata.data.named.timeoffirstobs = buffer;
    735       i = 1+snprintf(buffer, buffersize,
    736     "  %4d    %2d    %2d    %2d    %2d   %10.7f     GPS         "
    737     "TIME OF FIRST OBS", cti.year%100, cti.month, cti.day, cti.hour,
    738     cti.minute, cti.second + fmod(Parser->Data.timeofweek/1000.0,1.0));
    739 
    740     buffer += i; buffersize -= i;
    741   }
    742 
    743   hdata.numheaders = 11;
    744 
    745   if(Parser->headerfile)
    746   {
    747     FILE *fh;
    748     if((fh = fopen(Parser->headerfile, "r")))
    749     {
    750       int siz;
    751       char *lastblockstart;
    752       if((siz = fread(buffer, 1, buffersize-1, fh)) > 0)
    753       {
    754         buffer[siz] = '\n';
    755         if(siz == buffersize)
    756         {
    757           fprintf(stderr, "Header file is too large. Only %d bytes read.",
    758           siz);
    759         }
    760         /* scan the file line by line and enter the entries in the list */
    761         /* warn for "# / TYPES OF OBSERV" and "TIME OF FIRST OBS" */
    762         /* overwrites entries, except for comments */
    763         lastblockstart = buffer;
    764         for(i = 0; i < siz; ++i)
    765         {
    766           if(buffer[i] == '\n')
    767           { /* we found a line */
    768             char *end;
    769             while(buffer[i+1] == '\r')
    770               ++i; /* skip \r in case there are any */
    771             end = buffer+i;
    772             while(*end == '\t' || *end == ' ' || *end == '\r' || *end == '\n')
    773               *(end--) = 0;
    774             if(end-lastblockstart < 60+5) /* short line */
    775               fprintf(stderr, "Short Header line '%s' ignored.\n", lastblockstart);
    776             else
    777             {
    778               int pos;
    779               if(!strcmp("COMMENT", lastblockstart+60))
    780                 pos = hdata.numheaders;
    781               else
    782               {
    783                 for(pos = 0; pos < hdata.numheaders; ++pos)
    784                 {
    785                   if(!strcmp(hdata.data.unnamed[pos]+60, lastblockstart+60))
    786                     break;
    787                 }
    788                 if(!strcmp("# / TYPES OF OBSERV", lastblockstart+60)
    789                 || !strcmp("TIME OF FIRST OBS", lastblockstart+60))
    790                 {
    791                   fprintf(stderr, "Overwriting header '%s' is dangerous.\n",
    792                   lastblockstart+60);
    793                 }
    794               }
    795               if(pos >= MAXHEADERLINES)
    796               {
    797                 fprintf(stderr,
    798                 "Maximum number of header lines of %d reached.\n",
    799                 MAXHEADERLINES);
    800               }
    801               else if(!strcmp("END OF HEADER", lastblockstart+60))
    802               {
    803                 fprintf(stderr, "End of header ignored.\n");
    804               }
    805               else
    806               {
    807                 hdata.data.unnamed[pos] = lastblockstart;
    808                 if(pos == hdata.numheaders)
    809                   ++hdata.numheaders;
    810               }
    811             }
    812             lastblockstart = buffer+i+1;
    813           }
    814         }
    815       }
    816       else
    817       {
    818         fprintf(stderr, "Could not read data from headerfile '%s'.\n",
    819         Parser->headerfile);
    820       }
    821       fclose(fh);
    822     }
    823     else
    824     {
    825       fprintf(stderr, "Could not open header datafile '%s'.\n",
    826       Parser->headerfile);
    827     }
    828   }
    829 
    830   for(i = 0; i < hdata.numheaders; ++i)
    831     printf("%s\n", hdata.data.unnamed[i]);
    832   printf("                                                            "
    833   "END OF HEADER\n");
    834 }
    835 
    836 /* let the output complete a block if necessary */
    837 static void signalhandler(int sig)
    838 {
    839   if(!stop)
    840   {
    841     fprintf(stderr, "Stop signal number %d received. "
    842     "Trying to terminate gentle.\n", sig);
    843     stop = 1;
    844     alarm(1);
     37//
     38////////////////////////////////////////////////////////////////////////////
     39void rtcm3::Decode(char* buffer, int bufLen) {
     40  for (int ii = 0; ii < bufLen; ii++) {
     41    HandleByte(&_Parser, buffer[ii]);
    84542  }
    84643}
    847 
    848 static void signalhandler_alarm(int sig)
    849 {
    850   fprintf(stderr, "Programm forcefully terminated.\n");
    851   exit(1);
    852 }
    853 
    854 static void HandleByte(struct RTCM3ParserData *Parser, unsigned int byte)
    855 {
    856   Parser->Message[Parser->MessageSize++] = byte;
    857   if(Parser->MessageSize >= Parser->NeedBytes)
    858   {
    859     int r;
    860     while((r = RTCM3Parser(Parser)))
    861     {
    862       int i, j, o;
    863       struct converttimeinfo cti;
    864 
    865       if(!Parser->init)
    866       {
    867         HandleHeader(Parser);
    868         Parser->init = 1;
    869       }
    870       if(r == 2 && !Parser->validwarning)
    871       {
    872         printf("No valid RINEX! All values are modulo 299792.458!"
    873         "           COMMENT\n");
    874         Parser->validwarning = 1;
    875       }
    876 
    877       converttime(&cti, Parser->Data.week,
    878       floor(Parser->Data.timeofweek/1000.0));
    879       printf(" %02d %2d %2d %2d %2d %10.7f  0%3d",
    880       cti.year%100, cti.month, cti.day, cti.hour, cti.minute, cti.second
    881       + fmod(Parser->Data.timeofweek/1000.0,1.0), Parser->Data.numsats);
    882       for(i = 0; i < 12 && i < Parser->Data.numsats; ++i)
    883       {
    884         if(Parser->Data.satellites[i] <= PRN_GPS_END)
    885           printf("G%02d", Parser->Data.satellites[i]);
    886         else if(Parser->Data.satellites[i] >= PRN_GLONASS_START
    887         && Parser->Data.satellites[i] <= PRN_GLONASS_END)
    888           printf("R%02d", Parser->Data.satellites[i] - (PRN_GLONASS_START-1));
    889         else
    890           printf("%3d", Parser->Data.satellites[i]);
    891       }
    892       printf("\n");
    893       o = 12;
    894       j = Parser->Data.numsats - 12;
    895       while(j > 0)
    896       {
    897         printf("                                ");
    898         for(i = o; i < o+12 && i < Parser->Data.numsats; ++i)
    899         {
    900           if(Parser->Data.satellites[i] <= PRN_GPS_END)
    901             printf("G%02d", Parser->Data.satellites[i]);
    902           else if(Parser->Data.satellites[i] >= PRN_GLONASS_START
    903           && Parser->Data.satellites[i] <= PRN_GLONASS_END)
    904             printf("R%02d", Parser->Data.satellites[i] - (PRN_GLONASS_START-1));
    905           else
    906             printf("%3d", Parser->Data.satellites[i]);
    907         }
    908         printf("\n");
    909         j -= 12;
    910         o += 12;
    911       }
    912       for(i = 0; i < Parser->Data.numsats; ++i)
    913       {
    914         for(j = 0; j < Parser->numdatatypes; ++j)
    915         {
    916           if(!(Parser->Data.dataflags[i] & Parser->dataflag[j])
    917           || isnan(Parser->Data.measdata[i][Parser->datapos[j]])
    918           || isinf(Parser->Data.measdata[i][Parser->datapos[j]]))
    919           { /* no or illegal data */
    920             printf("                ");
    921           }
    922           else
    923           {
    924             char lli = ' ';
    925             char snr = ' ';
    926             if(Parser->dataflag[j] & (GNSSDF_L1CDATA|GNSSDF_L1PDATA))
    927             {
    928               if(Parser->Data.dataflags[i] & GNSSDF_LOCKLOSSL1)
    929                 lli = '1';
    930               snr = '0'+Parser->Data.snrL1[i];
    931             }
    932             if(Parser->dataflag[j] & (GNSSDF_L2CDATA|GNSSDF_L2PDATA))
    933             {
    934               if(Parser->Data.dataflags[i] & GNSSDF_LOCKLOSSL2)
    935                 lli = '1';
    936               snr = '0'+Parser->Data.snrL2[i];
    937             }
    938             printf("%14.3f%c%c",
    939             Parser->Data.measdata[i][Parser->datapos[j]],lli,snr);
    940           }
    941           if(j%5 == 4 || j == Parser->numdatatypes-1)
    942             printf("\n");
    943         }
    944       }
    945     }
    946   }
    947 }
    948 
    949 int main(int argc, char **argv)
    950 {
    951   struct Args args;
    952   struct RTCM3ParserData Parser;
    953 
    954   setbuf(stdout, 0);
    955   setbuf(stdin, 0);
    956   setbuf(stderr, 0);
    957 
    958   {
    959     char *a;
    960     int i=0;
    961     for(a = revisionstr+11; *a && *a != ' '; ++a)
    962       revisionstr[i++] = *a;
    963     revisionstr[i] = 0;
    964   }
    965 
    966   signal(SIGINT, signalhandler);
    967   signal(SIGALRM,signalhandler_alarm);
    968   signal(SIGQUIT,signalhandler);
    969   signal(SIGTERM,signalhandler);
    970   signal(SIGPIPE,signalhandler);
    971   memset(&Parser, 0, sizeof(Parser));
    972   {
    973     time_t tim;
    974     tim = time(0) - ((10*365+2+5)*24*60*60+LEAPSECONDS);
    975     Parser.GPSWeek = tim/(7*24*60*60);
    976     Parser.GPSTOW = tim%(7*24*60*60);
    977   }
    978 
    979   if(getargs(argc, argv, &args))
    980   {
    981     int i, sockfd, numbytes; 
    982     char buf[MAXDATASIZE];
    983     struct hostent *he;
    984     struct sockaddr_in their_addr; /* connector's address information */
    985 
    986     Parser.headerfile = args.headerfile;
    987 
    988     if(!(he=gethostbyname(args.server)))
    989     {
    990       perror("gethostbyname");
    991       exit(1);
    992     }
    993     if((sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1)
    994     {
    995       perror("socket");
    996       exit(1);
    997     }
    998     their_addr.sin_family = AF_INET;    /* host byte order */
    999     their_addr.sin_port = htons(args.port);  /* short, network byte order */
    1000     their_addr.sin_addr = *((struct in_addr *)he->h_addr);
    1001     memset(&(their_addr.sin_zero), '\0', 8);
    1002     if(connect(sockfd, (struct sockaddr *)&their_addr,
    1003     sizeof(struct sockaddr)) == -1)
    1004     {
    1005       perror("connect");
    1006       exit(1);
    1007     }
    1008 
    1009     if(!args.data)
    1010     {
    1011       i = snprintf(buf, MAXDATASIZE,
    1012       "GET / HTTP/1.0\r\n"
    1013       "User-Agent: %s/%s\r\n"
    1014 #ifdef UNUSED
    1015       "Accept: */*\r\n"
    1016       "Connection: close\r\n"
    1017 #endif
    1018       "\r\n"
    1019       , AGENTSTRING, revisionstr);
    1020     }
    1021     else
    1022     {
    1023       i=snprintf(buf, MAXDATASIZE-40, /* leave some space for login */
    1024       "GET /%s HTTP/1.0\r\n"
    1025       "User-Agent: %s/%s\r\n"
    1026 #ifdef UNUSED
    1027       "Accept: */*\r\n"
    1028       "Connection: close\r\n"
    1029 #endif
    1030       "Authorization: Basic "
    1031       , args.data, AGENTSTRING, revisionstr);
    1032       if(i > MAXDATASIZE-40 && i < 0) /* second check for old glibc */
    1033       {
    1034         fprintf(stderr, "Requested data too long\n");
    1035         exit(1);
    1036       }
    1037       i += encode(buf+i, MAXDATASIZE-i-5, args.user, args.password);
    1038       if(i > MAXDATASIZE-5)
    1039       {
    1040         fprintf(stderr, "Username and/or password too long\n");
    1041         exit(1);
    1042       }
    1043       snprintf(buf+i, 5, "\r\n\r\n");
    1044       i += 5;
    1045     }
    1046     if(send(sockfd, buf, (size_t)i, 0) != i)
    1047     {
    1048       perror("send");
    1049       exit(1);
    1050     }
    1051     if(args.data)
    1052     {
    1053       int k = 0;
    1054       while(!stop && (numbytes=recv(sockfd, buf, MAXDATASIZE-1, 0)) != -1)
    1055       {
    1056         if(!k)
    1057         {
    1058           if(numbytes < 12 || strncmp("ICY 200 OK\r\n", buf, 12))
    1059           {
    1060             fprintf(stderr, "Could not get the requested data: ");
    1061             for(k = 0; k < numbytes && buf[k] != '\n' && buf[k] != '\r'; ++k)
    1062             {
    1063               fprintf(stderr, "%c", isprint(buf[k]) ? buf[k] : '.');
    1064             }
    1065             fprintf(stderr, "\n");
    1066             exit(1);
    1067           }
    1068           ++k;
    1069         }
    1070         else
    1071         {
    1072           int z;
    1073           for(z = 0; z < numbytes && !stop; ++z)
    1074             HandleByte(&Parser, buf[z]);
    1075         }
    1076       }
    1077     }
    1078     else
    1079     {
    1080       while((numbytes=recv(sockfd, buf, MAXDATASIZE-1, 0)) > 0)
    1081       {
    1082         fwrite(buf, (size_t)numbytes, 1, stdout);
    1083       }
    1084     }
    1085 
    1086     close(sockfd);
    1087   }
    1088   return 0;
    1089 }
Note: See TracChangeset for help on using the changeset viewer.