diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm.cc b/src/algorithms/libs/rtklib/rtklib_rtcm.cc new file mode 100644 index 000000000..e49f98831 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm.cc @@ -0,0 +1,413 @@ +/*! + * \file rtklib_rtcm.cc + * \brief RTCM functions + * \authors + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#include "rtklib_rtcm.h" +#include "rtklib_rtcm2.h" +#include "rtklib_rtcm3.h" +#include "rtklib_rtkcmn.h" + +//extern int encode_rtcm3(rtcm_t *rtcm, int type, int sync); + + +/* initialize rtcm control ----------------------------------------------------- + * initialize rtcm control struct and reallocate memory for observation and + * ephemeris buffer in rtcm control struct + * args : rtcm_t *raw IO rtcm control struct + * return : status (1:ok,0:memory allocation error) + *-----------------------------------------------------------------------------*/ +int init_rtcm(rtcm_t *rtcm) +{ + gtime_t time0 = {0}; + obsd_t data0 = {{0}}; + eph_t eph0 = {0, -1, -1}; + geph_t geph0 = {0, -1}; + ssr_t ssr0 = {{{0}}}; + int i, j; + + trace(3, "init_rtcm:\n"); + + rtcm->staid = rtcm->stah = rtcm->seqno = rtcm->outtype = 0; + rtcm->time = rtcm->time_s = time0; + rtcm->sta.name[0] = rtcm->sta.marker[0] = '\0'; + rtcm->sta.antdes[0] = rtcm->sta.antsno[0] = '\0'; + rtcm->sta.rectype[0] = rtcm->sta.recver[0] = rtcm->sta.recsno[0] = '\0'; + rtcm->sta.antsetup = rtcm->sta.itrf = rtcm->sta.deltype = 0; + for (i = 0;i < 3;i++) + { + rtcm->sta.pos[i] = rtcm->sta.del[i] = 0.0; + } + rtcm->sta.hgt = 0.0; + rtcm->dgps = NULL; + for (i = 0;i < MAXSAT;i++) + { + rtcm->ssr[i] = ssr0; + } + rtcm->msg[0] = rtcm->msgtype[0] = rtcm->opt[0] = '\0'; + for (i = 0;i < 6;i++) rtcm->msmtype[i][0] = '\0'; + rtcm->obsflag = rtcm->ephsat = 0; + for (i = 0;i < MAXSAT;i++) for (j = 0;j < NFREQ+NEXOBS;j++) + { + rtcm->cp[i][j] = 0.0; + rtcm->lock[i][j] = rtcm->loss[i][j] = 0; + rtcm->lltime[i][j] = time0; + } + rtcm->nbyte = rtcm->nbit = rtcm->len = 0; + rtcm->word = 0; + for (i = 0;i < 100;i++) rtcm->nmsg2[i] = 0; + for (i = 0;i < 300;i++) rtcm->nmsg3[i] = 0; + + rtcm->obs.data = NULL; + rtcm->nav.eph = NULL; + rtcm->nav.geph = NULL; + + /* reallocate memory for observation and ephemris buffer */ + if (!(rtcm->obs.data = (obsd_t *)malloc(sizeof(obsd_t)*MAXOBS)) || + !(rtcm->nav.eph = (eph_t *)malloc(sizeof(eph_t )*MAXSAT)) || + !(rtcm->nav.geph = (geph_t *)malloc(sizeof(geph_t)*MAXPRNGLO))) + { + free_rtcm(rtcm); + return 0; + } + rtcm->obs.n = 0; + rtcm->nav.n = MAXSAT; + rtcm->nav.ng = MAXPRNGLO; + for (i = 0;i < MAXOBS ;i++) rtcm->obs.data[i] = data0; + for (i = 0;i < MAXSAT ;i++) rtcm->nav.eph [i] = eph0; + for (i = 0;i < MAXPRNGLO;i++) rtcm->nav.geph[i] = geph0; + return 1; +} + + +/* free rtcm control ---------------------------------------------------------- + * free observation and ephemris buffer in rtcm control struct + * args : rtcm_t *raw IO rtcm control struct + * return : none + *-----------------------------------------------------------------------------*/ +void free_rtcm(rtcm_t *rtcm) +{ + trace(3, "free_rtcm:\n"); + + /* free memory for observation and ephemeris buffer */ + free(rtcm->obs.data); rtcm->obs.data = NULL; rtcm->obs.n = 0; + free(rtcm->nav.eph ); rtcm->nav.eph = NULL; rtcm->nav.n = 0; + free(rtcm->nav.geph); rtcm->nav.geph = NULL; rtcm->nav.ng = 0; +} + + +/* input rtcm 2 message from stream -------------------------------------------- + * fetch next rtcm 2 message and input a message from byte stream + * args : rtcm_t *rtcm IO rtcm control struct + * unsigned char data I stream data (1 byte) + * return : status (-1: error message, 0: no message, 1: input observation data, + * 2: input ephemeris, 5: input station pos/ant parameters, + * 6: input time parameter, 7: input dgps corrections, + * 9: input special message) + * notes : before firstly calling the function, time in rtcm control struct has + * to be set to the approximate time within 1/2 hour in order to resolve + * ambiguity of time in rtcm messages. + * supported msgs RTCM ver.2: 1,3,9,14,16,17,18,19,22 + * refer [1] for RTCM ver.2 + *-----------------------------------------------------------------------------*/ +int input_rtcm2(rtcm_t *rtcm, unsigned char data) +{ + unsigned char preamb; + int i; + + trace(5, "input_rtcm2: data=%02x\n", data); + + if ((data & 0xC0) != 0x40) return 0; /* ignore if upper 2bit != 01 */ + + for (i = 0; i < 6; i++, data >>= 1) + { /* decode 6-of-8 form */ + rtcm->word = (rtcm->word << 1) + (data & 1); + + /* synchronize frame */ + if (rtcm->nbyte == 0) + { + preamb = (unsigned char)(rtcm->word>>22); + if (rtcm->word & 0x40000000) preamb ^= 0xFF; /* decode preamble */ + if (preamb != RTCM2PREAMB) continue; + + /* check parity */ + if (!decode_word(rtcm->word, rtcm->buff)) continue; + rtcm->nbyte = 3; rtcm->nbit = 0; + continue; + } + if (++rtcm->nbit < 30) continue; else rtcm->nbit = 0; + + /* check parity */ + if (!decode_word(rtcm->word, rtcm->buff+rtcm->nbyte)) + { + trace(2, "rtcm2 partity error: i=%d word=%08x\n", i, rtcm->word); + rtcm->nbyte = 0; rtcm->word &= 0x3; + continue; + } + rtcm->nbyte += 3; + if (rtcm->nbyte == 6) rtcm->len = (rtcm->buff[5]>>3)*3+6; + if (rtcm->nbyte < rtcm->len) continue; + rtcm->nbyte = 0; rtcm->word &= 0x3; + + /* decode rtcm2 message */ + return decode_rtcm2(rtcm); + } + return 0; +} + + +/* input rtcm 3 message from stream -------------------------------------------- + * fetch next rtcm 3 message and input a message from byte stream + * args : rtcm_t *rtcm IO rtcm control struct + * unsigned char data I stream data (1 byte) + * return : status (-1: error message, 0: no message, 1: input observation data, + * 2: input ephemeris, 5: input station pos/ant parameters, + * 10: input ssr messages) + * notes : before firstly calling the function, time in rtcm control struct has + * to be set to the approximate time within 1/2 week in order to resolve + * ambiguity of time in rtcm messages. + * + * to specify input options, set rtcm->opt to the following option + * strings separated by spaces. + * + * -EPHALL : input all ephemerides + * -STA=nnn : input only message with STAID=nnn + * -GLss : select signal ss for GPS MSM (ss=1C,1P,...) + * -RLss : select signal ss for GLO MSM (ss=1C,1P,...) + * -ELss : select signal ss for GAL MSM (ss=1C,1B,...) + * -JLss : select signal ss for QZS MSM (ss=1C,2C,...) + * -CLss : select signal ss for BDS MSM (ss=2I,7I,...) + * + * supported RTCM 3 messages + * (ref [2][3][4][5][6][7][8][9][10][11][12][13][14][15]) + * + * TYPE GPS GLOASS GALILEO QZSS BEIDOU SBAS + * ---------------------------------------------------------------------- + * OBS C-L1 : 1001~ 1009~ - - - - + * F-L1 : 1002 1010 - - - - + * C-L12 : 1003~ 1011~ - - - - + * F-L12 : 1004 1012 - - - - + * + * NAV : 1019 1020 1045* 1044* 1047* - + * - - 1046* - - - + * + * MSM 1 : 1071~ 1081~ 1091~ 1111*~ 1121*~ 1101*~ + * 2 : 1072~ 1082~ 1092~ 1112*~ 1122*~ 1102*~ + * 3 : 1073~ 1083~ 1093~ 1113*~ 1123*~ 1103*~ + * 4 : 1074 1084 1094 1114* 1124* 1104* + * 5 : 1075 1085 1095 1115* 1125* 1105* + * 6 : 1076 1086 1096 1116* 1126* 1106* + * 7 : 1077 1087 1097 1117* 1127* 1107* + * + * SSR OBT : 1057 1063 1240* 1246* 1258* - + * CLK : 1058 1064 1241* 1247* 1259* - + * BIAS : 1059 1065 1242* 1248* 1260* - + * OBTCLK: 1060 1066 1243* 1249* 1261* - + * URA : 1061 1067 1244* 1250* 1262* - + * HRCLK : 1062 1068 1245* 1251* 1263* - + * + * ANT INFO : 1005 1006 1007 1008 1033 + * ---------------------------------------------------------------------- + * (* draft, ~ only encode) + * + * for MSM observation data with multiple signals for a frequency, + * a signal is selected according to internal priority. to select + * a specified signal, use the input options. + * + * rtcm3 message format: + * +----------+--------+-----------+--------------------+----------+ + * | preamble | 000000 | length | data message | parity | + * +----------+--------+-----------+--------------------+----------+ + * |<-- 8 --->|<- 6 -->|<-- 10 --->|<--- length x 8 --->|<-- 24 -->| + * + *-----------------------------------------------------------------------------*/ +int input_rtcm3(rtcm_t *rtcm, unsigned char data) +{ + trace(5, "input_rtcm3: data=%02x\n", data); + + /* synchronize frame */ + if (rtcm->nbyte == 0) + { + if (data != RTCM3PREAMB) return 0; + rtcm->buff[rtcm->nbyte++] = data; + return 0; + } + rtcm->buff[rtcm->nbyte++] = data; + + if (rtcm->nbyte == 3) + { + rtcm->len = getbitu(rtcm->buff, 14, 10)+3; /* length without parity */ + } + if (rtcm->nbyte < 3 || rtcm->nbyte < rtcm->len+3) return 0; + rtcm->nbyte = 0; + + /* check parity */ + if (rtk_crc24q(rtcm->buff, rtcm->len) != getbitu(rtcm->buff, rtcm->len*8, 24)) + { + trace(2, "rtcm3 parity error: len=%d\n", rtcm->len); + return 0; + } + /* decode rtcm3 message */ + return decode_rtcm3(rtcm); +} + + +/* input rtcm 2 message from file ---------------------------------------------- + * fetch next rtcm 2 message and input a messsage from file + * args : rtcm_t *rtcm IO rtcm control struct + * FILE *fp I file pointer + * return : status (-2: end of file, -1...10: same as above) + * notes : same as above + *-----------------------------------------------------------------------------*/ +int input_rtcm2f(rtcm_t *rtcm, FILE *fp) +{ + int i, data = 0, ret; + + trace(4, "input_rtcm2f: data=%02x\n", data); + + for (i = 0;i < 4096;i++) + { + if ((data = fgetc(fp)) == EOF) return -2; + if ((ret = input_rtcm2(rtcm, (unsigned char)data))) return ret; + } + return 0; /* return at every 4k bytes */ +} + + +/* input rtcm 3 message from file ---------------------------------------------- + * fetch next rtcm 3 message and input a messsage from file + * args : rtcm_t *rtcm IO rtcm control struct + * FILE *fp I file pointer + * return : status (-2: end of file, -1...10: same as above) + * notes : same as above + *-----------------------------------------------------------------------------*/ +int input_rtcm3f(rtcm_t *rtcm, FILE *fp) +{ + int i, data = 0, ret; + + trace(4, "input_rtcm3f: data=%02x\n", data); + + for (i = 0;i < 4096;i++) + { + if ((data = fgetc(fp)) == EOF) return -2; + if ((ret = input_rtcm3(rtcm, (unsigned char)data))) return ret; + } + return 0; /* return at every 4k bytes */ +} + + +/* generate rtcm 2 message ----------------------------------------------------- + * generate rtcm 2 message + * args : rtcm_t *rtcm IO rtcm control struct + * int type I message type + * int sync I sync flag (1:another message follows) + * return : status (1:ok,0:error) + *-----------------------------------------------------------------------------*/ +int gen_rtcm2(rtcm_t *rtcm, int type, int sync) +{ + trace(4, "gen_rtcm2: type=%d sync=%d\n", type, sync); + + rtcm->nbit = rtcm->len = rtcm->nbyte = 0; + + /* not yet implemented */ + + return 0; +} + + +///* generate rtcm 3 message ----------------------------------------------------- +// * generate rtcm 3 message +// * args : rtcm_t *rtcm IO rtcm control struct +// * int type I message type +// * int sync I sync flag (1:another message follows) +// * return : status (1:ok,0:error) +// *-----------------------------------------------------------------------------*/ +//int gen_rtcm3(rtcm_t *rtcm, int type, int sync) +//{ +// unsigned int crc; +// int i = 0; +// +// trace(4, "gen_rtcm3: type=%d sync=%d\n", type, sync); +// +// rtcm->nbit = rtcm->len = rtcm->nbyte = 0; +// +// /* set preamble and reserved */ +// setbitu(rtcm->buff, i, 8, RTCM3PREAMB); i += 8; +// setbitu(rtcm->buff, i, 6, 0 ); i += 6; +// setbitu(rtcm->buff, i, 10, 0 ); i += 10; +// +// /* encode rtcm 3 message body */ +// if (!encode_rtcm3(rtcm, type, sync)) return 0; +// +// /* padding to align 8 bit boundary */ +// for (i = rtcm->nbit;i%8;i++) +// { +// setbitu(rtcm->buff, i, 1, 0); +// } +// /* message length (header+data) (bytes) */ +// if ((rtcm->len = i/8) >= 3+1024) +// { +// trace(2, "generate rtcm 3 message length error len=%d\n", rtcm->len-3); +// rtcm->nbit = rtcm->len = 0; +// return 0; +// } +// /* message length without header and parity */ +// setbitu(rtcm->buff, 14, 10, rtcm->len-3); +// +// /* crc-24q */ +// crc = rtk_crc24q(rtcm->buff, rtcm->len); +// setbitu(rtcm->buff, i, 24, crc); +// +// /* length total (bytes) */ +// rtcm->nbyte = rtcm->len+3; +// +// return 1; +//} diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm.h b/src/algorithms/libs/rtklib/rtklib_rtcm.h new file mode 100644 index 000000000..910a4e79b --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm.h @@ -0,0 +1,74 @@ +/*! + * \file rtklib_rtcm.h + * \brief RTCM functions headers + * \authors + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#include "rtklib.h" + + +#ifndef GNSS_SDR_RTKLIB_RTCM_H_ +#define GNSS_SDR_RTKLIB_RTCM_H_ + +#define RTCM2PREAMB 0x66 /* rtcm ver.2 frame preamble */ +#define RTCM3PREAMB 0xD3 /* rtcm ver.3 frame preamble */ + + +int init_rtcm(rtcm_t *rtcm); +void free_rtcm(rtcm_t *rtcm); +int input_rtcm2(rtcm_t *rtcm, unsigned char data); +int input_rtcm3(rtcm_t *rtcm, unsigned char data); +int input_rtcm2f(rtcm_t *rtcm, FILE *fp); +int input_rtcm3f(rtcm_t *rtcm, FILE *fp); +int gen_rtcm2(rtcm_t *rtcm, int type, int sync); +//int gen_rtcm3(rtcm_t *rtcm, int type, int sync); + + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm2.cc b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc new file mode 100644 index 000000000..3e036e3c3 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm2.cc @@ -0,0 +1,548 @@ +/*! + * \file rtklib_rtcm2.cc + * \brief RTCM functions for v2 + * \authors + * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + + + +#include "rtklib_rtcm2.h" +#include "rtklib_rtcm3.h" +#include "rtklib_rtkcmn.h" + + +/* adjust hourly rollover of rtcm 2 time -------------------------------------*/ +void adjhour(rtcm_t *rtcm, double zcnt) +{ + double tow,hour,sec; + int week; + + /* if no time, get cpu time */ + if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); + tow = time2gpst(rtcm->time,&week); + hour = floor(tow/3600.0); + sec = tow-hour*3600.0; + if (zcntsec+1800.0) zcnt-=3600.0; + rtcm->time = gpst2time(week,hour*3600+zcnt); +} + + +/* get observation data index ------------------------------------------------*/ +int obsindex(obs_t *obs, gtime_t time, int sat) +{ + int i,j; + + for (i = 0;in;i++) + { + if (obs->data[i].sat == sat) return i; /* field already exists */ + } + if (i>=MAXOBS) return -1; /* overflow */ + + /* add new field */ + obs->data[i].time = time; + obs->data[i].sat = sat; + for (j = 0;jdata[i].L[j] = obs->data[i].P[j] = 0.0; + obs->data[i].D[j] = 0.0; + obs->data[i].SNR[j] = obs->data[i].LLI[j] = obs->data[i].code[j] = 0; + } + obs->n++; + return i; +} + + +/* decode type 1/9: differential gps correction/partial correction set -------*/ +int decode_type1(rtcm_t *rtcm) +{ + int i = 48,fact,udre,prn,sat,iod; + double prc,rrc; + + trace(4,"decode_type1: len=%d\n",rtcm->len); + + while (i+40<=rtcm->len*8) + { + fact = getbitu(rtcm->buff,i, 1); i += 1; + udre = getbitu(rtcm->buff,i, 2); i += 2; + prn = getbitu(rtcm->buff,i, 5); i += 5; + prc = getbits(rtcm->buff,i,16); i += 16; + rrc = getbits(rtcm->buff,i, 8); i += 8; + iod = getbits(rtcm->buff,i, 8); i += 8; + if (prn == 0) prn = 32; + if (prc == 0x80000000||rrc == 0xFFFF8000) + { + trace(2,"rtcm2 1 prc/rrc indicates satellite problem: prn=%d\n",prn); + continue; + } + if (rtcm->dgps) + { + sat = satno(SYS_GPS,prn); + rtcm->dgps[sat-1].t0 = rtcm->time; + rtcm->dgps[sat-1].prc = prc*(fact?0.32:0.02); + rtcm->dgps[sat-1].rrc = rrc*(fact?0.032:0.002); + rtcm->dgps[sat-1].iod = iod; + rtcm->dgps[sat-1].udre = udre; + } + } + return 7; +} + + +/* decode type 3: reference station parameter --------------------------------*/ +int decode_type3(rtcm_t *rtcm) +{ + int i = 48; + + trace(4,"decode_type3: len=%d\n",rtcm->len); + + if (i+96<=rtcm->len*8) + { + rtcm->sta.pos[0] = getbits(rtcm->buff,i,32)*0.01; i += 32; + rtcm->sta.pos[1] = getbits(rtcm->buff,i,32)*0.01; i += 32; + rtcm->sta.pos[2] = getbits(rtcm->buff,i,32)*0.01; + } + else + { + trace(2,"rtcm2 3 length error: len=%d\n",rtcm->len); + return -1; + } + return 5; +} + + +/* decode type 14: gps time of week ------------------------------------------*/ +int decode_type14(rtcm_t *rtcm) +{ + double zcnt; + int i = 48,week,hour,leaps; + + trace(4,"decode_type14: len=%d\n",rtcm->len); + + zcnt = getbitu(rtcm->buff,24,13); + if (i+24<=rtcm->len*8) + { + week = getbitu(rtcm->buff,i,10); i += 10; + hour = getbitu(rtcm->buff,i, 8); i += 8; + leaps = getbitu(rtcm->buff,i, 6); + } + else + { + trace(2,"rtcm2 14 length error: len=%d\n",rtcm->len); + return -1; + } + week = adjgpsweek(week); + rtcm->time = gpst2time(week,hour*3600.0+zcnt*0.6); + rtcm->nav.leaps = leaps; + return 6; +} + + +/* decode type 16: gps special message ---------------------------------------*/ +int decode_type16(rtcm_t *rtcm) +{ + int i = 48,n = 0; + + trace(4,"decode_type16: len=%d\n",rtcm->len); + + while (i+8<=rtcm->len*8&&n<90) + { + rtcm->msg[n++] = getbitu(rtcm->buff,i,8); i += 8; + } + rtcm->msg[n] = '\0'; + + trace(3,"rtcm2 16 message: %s\n",rtcm->msg); + return 9; +} + + +/* decode type 17: gps ephemerides -------------------------------------------*/ +int decode_type17(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc,sqrtA; + int i = 48,week,prn,sat; + + trace(4,"decode_type17: len=%d\n",rtcm->len); + + if (i+480<=rtcm->len*8) + { + week = getbitu(rtcm->buff,i,10); i += 10; + eph.idot = getbits(rtcm->buff,i,14)*TWO_N43*SC2RAD; i += 14; + eph.iode = getbitu(rtcm->buff,i, 8); i += 8; + toc = getbitu(rtcm->buff,i,16)*16.0; i += 16; + eph.f1 = getbits(rtcm->buff,i,16)*TWO_N43; i += 16; + eph.f2 = getbits(rtcm->buff,i, 8)*TWO_N55; i += 8; + eph.crs = getbits(rtcm->buff,i,16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff,i,16)*TWO_N43*SC2RAD; i += 16; + eph.cuc = getbits(rtcm->buff,i,16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff,i,32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff,i,16); i += 16; + sqrtA = getbitu(rtcm->buff,i,32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff,i,16); i += 16; + eph.OMG0 = getbits(rtcm->buff,i,32)*TWO_N31*SC2RAD; i += 32; + eph.cic = getbits(rtcm->buff,i,16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff,i,32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff,i,16)*TWO_N29; i += 16; + eph.omg = getbits(rtcm->buff,i,32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff,i,16)*TWO_N5; i += 16; + eph.OMGd = getbits(rtcm->buff,i,24)*TWO_N43*SC2RAD; i += 24; + eph.M0 = getbits(rtcm->buff,i,32)*TWO_N31*SC2RAD; i += 32; + eph.iodc = getbitu(rtcm->buff,i,10); i += 10; + eph.f0 = getbits(rtcm->buff,i,22)*TWO_N31; i += 22; + prn = getbitu(rtcm->buff,i, 5); i += 5+3; + eph.tgd[0] = getbits(rtcm->buff,i, 8)*TWO_N31; i += 8; + eph.code = getbitu(rtcm->buff,i, 2); i += 2; + eph.sva = getbitu(rtcm->buff,i, 4); i += 4; + eph.svh = getbitu(rtcm->buff,i, 6); i += 6; + eph.flag = getbitu(rtcm->buff,i, 1); + } + else + { + trace(2,"rtcm2 17 length error: len=%d\n",rtcm->len); + return -1; + } + if (prn == 0) prn = 32; + sat = satno(SYS_GPS,prn); + eph.sat = sat; + eph.week = adjgpsweek(week); + eph.toe = gpst2time(eph.week,eph.toes); + eph.toc = gpst2time(eph.week,toc); + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/ +int decode_type18(rtcm_t *rtcm) +{ + gtime_t time; + double usec,cp,tt; + int i = 48,index,freq,sync = 1,code,sys,prn,sat,loss; + + trace(4,"decode_type18: len=%d\n",rtcm->len); + + if (i+24<=rtcm->len*8) + { + freq = getbitu(rtcm->buff,i, 2); i += 2+2; + usec = getbitu(rtcm->buff,i,20); i += 20; + } + else + { + trace(2,"rtcm2 18 length error: len=%d\n",rtcm->len); + return -1; + } + if (freq&0x1) + { + trace(2,"rtcm2 18 not supported frequency: freq=%d\n",freq); + return -1; + } + freq>>=1; + + while (i+48<=rtcm->len*8&&rtcm->obs.nbuff,i, 1); i += 1; + code = getbitu(rtcm->buff,i, 1); i += 1; + sys = getbitu(rtcm->buff,i, 1); i += 1; + prn = getbitu(rtcm->buff,i, 5); i += 5+3; + loss = getbitu(rtcm->buff,i, 5); i += 5; + cp = getbits(rtcm->buff,i,32); i += 32; + if (prn == 0) prn = 32; + if (!(sat = satno(sys?SYS_GLO:SYS_GPS,prn))) + { + trace(2,"rtcm2 18 satellite number error: sys=%d prn=%d\n",sys,prn); + continue; + } + time = timeadd(rtcm->time,usec*1E-6); + if (sys) time = utc2gpst(time); /* convert glonass time to gpst */ + + tt = timediff(rtcm->obs.data[0].time,time); + if (rtcm->obsflag||fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs,time,sat)) >= 0) + { + rtcm->obs.data[index].L[freq] = -cp/256.0; + rtcm->obs.data[index].LLI[freq] = rtcm->loss[sat-1][freq]!=loss; + rtcm->obs.data[index].code[freq] = + !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); + rtcm->loss[sat-1][freq] = loss; + } + } + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 19: rtk uncorrected pseudorange -------------------------------*/ +int decode_type19(rtcm_t *rtcm) +{ + gtime_t time; + double usec,pr,tt; + int i = 48,index,freq,sync = 1,code,sys,prn,sat; + + trace(4,"decode_type19: len=%d\n",rtcm->len); + + if (i+24<=rtcm->len*8) + { + freq = getbitu(rtcm->buff,i, 2); i += 2+2; + usec = getbitu(rtcm->buff,i,20); i += 20; + } + else + { + trace(2,"rtcm2 19 length error: len=%d\n",rtcm->len); + return -1; + } + if (freq&0x1) + { + trace(2,"rtcm2 19 not supported frequency: freq=%d\n",freq); + return -1; + } + freq>>=1; + + while (i+48<=rtcm->len*8&&rtcm->obs.nbuff,i, 1); i += 1; + code = getbitu(rtcm->buff,i, 1); i += 1; + sys = getbitu(rtcm->buff,i, 1); i += 1; + prn = getbitu(rtcm->buff,i, 5); i += 5+8; + pr = getbitu(rtcm->buff,i,32); i += 32; + if (prn == 0) prn = 32; + if (!(sat = satno(sys?SYS_GLO:SYS_GPS,prn))) + { + trace(2,"rtcm2 19 satellite number error: sys=%d prn=%d\n",sys,prn); + continue; + } + time = timeadd(rtcm->time,usec*1E-6); + if (sys) time = utc2gpst(time); /* convert glonass time to gpst */ + + tt = timediff(rtcm->obs.data[0].time,time); + if (rtcm->obsflag||fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs,time,sat))>=0) + { + rtcm->obs.data[index].P[freq] = pr*0.02; + rtcm->obs.data[index].code[freq] = + !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); + } + } + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 22: extended reference station parameter ----------------------*/ +int decode_type22(rtcm_t *rtcm) +{ + double del[2][3] = {{0}},hgt = 0.0; + int i = 48,j,noh; + + trace(4,"decode_type22: len=%d\n",rtcm->len); + + if (i+24<=rtcm->len*8) + { + del[0][0] = getbits(rtcm->buff,i,8)/25600.0; i += 8; + del[0][1] = getbits(rtcm->buff,i,8)/25600.0; i += 8; + del[0][2] = getbits(rtcm->buff,i,8)/25600.0; i += 8; + } + else + { + trace(2,"rtcm2 22 length error: len=%d\n",rtcm->len); + return -1; + } + if (i+24<=rtcm->len*8) + { + i += 5; noh = getbits(rtcm->buff,i,1); i += 1; + hgt = noh?0.0:getbitu(rtcm->buff,i,18)/25600.0; + i += 18; + } + if (i+24<=rtcm->len*8) + { + del[1][0] = getbits(rtcm->buff,i,8)/1600.0; i += 8; + del[1][1] = getbits(rtcm->buff,i,8)/1600.0; i += 8; + del[1][2] = getbits(rtcm->buff,i,8)/1600.0; + } + rtcm->sta.deltype = 1; /* xyz */ + for (j = 0;j<3;j++) rtcm->sta.del[j] = del[0][j]; + rtcm->sta.hgt = hgt; + return 5; +} + + +/* decode type 23: antenna type definition record ----------------------------*/ +int decode_type23(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 24: antenna reference point (arp) -----------------------------*/ +int decode_type24(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 31: differential glonass correction ---------------------------*/ +int decode_type31(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 32: differential glonass reference station parameters ---------*/ +int decode_type32(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 34: glonass partial differential correction set ---------------*/ +int decode_type34(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 36: glonass special message -----------------------------------*/ +int decode_type36(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 37: gnss system time offset -----------------------------------*/ +int decode_type37(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 59: proprietary message ---------------------------------------*/ +int decode_type59(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode rtcm ver.2 message -------------------------------------------------*/ +int decode_rtcm2(rtcm_t *rtcm) +{ + double zcnt; + int staid,seqno,stah,ret = 0,type = getbitu(rtcm->buff,8,6); + + trace(3,"decode_rtcm2: type=%2d len=%3d\n",type,rtcm->len); + + if ((zcnt = getbitu(rtcm->buff,24,13)*0.6)>=3600.0) + { + trace(2,"rtcm2 modified z-count error: zcnt=%.1f\n",zcnt); + return -1; + } + adjhour(rtcm,zcnt); + staid = getbitu(rtcm->buff,14,10); + seqno = getbitu(rtcm->buff,37, 3); + stah = getbitu(rtcm->buff,45, 3); + if (seqno-rtcm->seqno!=1&&seqno-rtcm->seqno!=-7) + { + trace(2,"rtcm2 message outage: seqno=%d->%d\n",rtcm->seqno,seqno); + } + rtcm->seqno = seqno; + rtcm->stah = stah; + + if (rtcm->outtype) + { + sprintf(rtcm->msgtype,"RTCM %2d (%4d) zcnt=%7.1f staid=%3d seqno=%d", + type,rtcm->len,zcnt,staid,seqno); + } + if (type == 3||type == 22||type == 23||type == 24) + { + if (rtcm->staid!=0&&staid!=rtcm->staid) + { + trace(2,"rtcm2 station id changed: %d->%d\n",rtcm->staid,staid); + } + rtcm->staid = staid; + } + if (rtcm->staid!=0&&staid!=rtcm->staid) + { + trace(2,"rtcm2 station id invalid: %d %d\n",staid,rtcm->staid); + return -1; + } + switch (type) + { + case 1: ret = decode_type1 (rtcm); break; + case 3: ret = decode_type3 (rtcm); break; + case 9: ret = decode_type1 (rtcm); break; + case 14: ret = decode_type14(rtcm); break; + case 16: ret = decode_type16(rtcm); break; + case 17: ret = decode_type17(rtcm); break; + case 18: ret = decode_type18(rtcm); break; + case 19: ret = decode_type19(rtcm); break; + case 22: ret = decode_type22(rtcm); break; + case 23: ret = decode_type23(rtcm); break; /* not supported */ + case 24: ret = decode_type24(rtcm); break; /* not supported */ + case 31: ret = decode_type31(rtcm); break; /* not supported */ + case 32: ret = decode_type32(rtcm); break; /* not supported */ + case 34: ret = decode_type34(rtcm); break; /* not supported */ + case 36: ret = decode_type36(rtcm); break; /* not supported */ + case 37: ret = decode_type37(rtcm); break; /* not supported */ + case 59: ret = decode_type59(rtcm); break; /* not supported */ + } + if (ret>=0) + { + if (1<=type&&type<=99) rtcm->nmsg2[type]++; else rtcm->nmsg2[0]++; + } + return ret; +} diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm2.h b/src/algorithms/libs/rtklib/rtklib_rtcm2.h new file mode 100644 index 000000000..56fbe948f --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm2.h @@ -0,0 +1,80 @@ +/*! + * \file rtklib_rtcm2.h + * \brief RTCM v2 functions headers + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RTCM2_H_ +#define GNSS_SDR_RTKLIB_RTCM2_H_ + +#include "rtklib.h" + + +void adjhour(rtcm_t *rtcm, double zcnt); +int obsindex(obs_t *obs, gtime_t time, int sat); +int decode_type1(rtcm_t *rtcm); +int decode_type3(rtcm_t *rtcm); +int decode_type14(rtcm_t *rtcm); +int decode_type16(rtcm_t *rtcm); +int decode_type17(rtcm_t *rtcm); +int decode_type18(rtcm_t *rtcm); +int decode_type19(rtcm_t *rtcm); +int decode_type22(rtcm_t *rtcm); +int decode_type23(rtcm_t *rtcm); +int decode_type24(rtcm_t *rtcm); +int decode_type31(rtcm_t *rtcm); +int decode_type32(rtcm_t *rtcm); +int decode_type34(rtcm_t *rtcm); +int decode_type36(rtcm_t *rtcm); +int decode_type37(rtcm_t *rtcm); +int decode_type59(rtcm_t *rtcm); +int decode_rtcm2(rtcm_t *rtcm); + +#endif diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm3.cc b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc new file mode 100644 index 000000000..90548e890 --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm3.cc @@ -0,0 +1,2727 @@ +/*! + * \file rtklib_rtcm3.cc + * \brief RTCM functions for v3 + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + + +#include "rtklib_rtcm3.h" +#include "rtklib_rtkcmn.h" + + + + +/* get sign-magnitude bits ---------------------------------------------------*/ +double getbitg(const unsigned char *buff, int pos, int len) +{ + double value = getbitu(buff, pos+1, len-1); + return getbitu(buff, pos, 1) ? -value : value; +} + + +/* adjust weekly rollover of gps time ----------------------------------------*/ +void adjweek(rtcm_t *rtcm, double tow) +{ + double tow_p; + int week; + + /* if no time, get cpu time */ + if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); + tow_p = time2gpst(rtcm->time, &week); + if (tow < tow_p-302400.0) tow += 604800.0; + else if (tow > tow_p+302400.0) tow -= 604800.0; + rtcm->time = gpst2time(week, tow); +} + + +/* adjust weekly rollover of bdt time ----------------------------------------*/ +int adjbdtweek(int week) +{ + int w; + (void)time2bdt(gpst2bdt(utc2gpst(timeget())), &w); + if (w < 1) w = 1; /* use 2006/1/1 if time is earlier than 2006/1/1 */ + return week+(w-week+512)/1024*1024; +} + + +/* adjust daily rollover of glonass time -------------------------------------*/ +void adjday_glot(rtcm_t *rtcm, double tod) +{ + gtime_t time; + double tow, tod_p; + int week; + + if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); + time = timeadd(gpst2utc(rtcm->time), 10800.0); /* glonass time */ + tow = time2gpst(time, &week); + tod_p = fmod(tow, 86400.0); tow -= tod_p; + if (tod < tod_p-43200.0) tod += 86400.0; + else if (tod > tod_p+43200.0) tod -= 86400.0; + time = gpst2time(week, tow+tod); + rtcm->time = utc2gpst(timeadd(time, -10800.0)); +} + + +/* adjust carrier-phase rollover ---------------------------------------------*/ +double adjcp(rtcm_t *rtcm, int sat, int freq, double cp) +{ + if (rtcm->cp[sat-1][freq] == 0.0) ; + else if (cp < rtcm->cp[sat-1][freq]-750.0) cp += 1500.0; + else if (cp > rtcm->cp[sat-1][freq]+750.0) cp -= 1500.0; + rtcm->cp[sat-1][freq] = cp; + return cp; +} + + +/* loss-of-lock indicator ----------------------------------------------------*/ +int lossoflock(rtcm_t *rtcm, int sat, int freq, int lock) +{ + int lli = (!lock && !rtcm->lock[sat-1][freq]) || lock < rtcm->lock[sat-1][freq]; + rtcm->lock[sat-1][freq] = (unsigned short)lock; + return lli; +} + + +/* s/n ratio -----------------------------------------------------------------*/ +unsigned char snratio(double snr) +{ + return (unsigned char)(snr <= 0.0 || 255.5 <= snr?0.0:snr*4.0+0.5); +} + + +/* get observation data index ------------------------------------------------*/ +int obsindex(obs_t *obs, gtime_t time, int sat) +{ + int i, j; + + for (i = 0;i < obs->n;i++) + { + if (obs->data[i].sat == sat) return i; /* field already exists */ + } + if (i >= MAXOBS) return -1; /* overflow */ + + /* add new field */ + obs->data[i].time = time; + obs->data[i].sat = sat; + for (j = 0;j < NFREQ+NEXOBS;j++) + { + obs->data[i].L[j] = obs->data[i].P[j] = 0.0; + obs->data[i].D[j] = 0.0; + obs->data[i].SNR[j] = obs->data[i].LLI[j] = obs->data[i].code[j] = 0; + } + obs->n++; + return i; +} + + +/* test station id consistency -----------------------------------------------*/ +int test_staid(rtcm_t *rtcm, int staid) +{ + char *p; + int type, id; + + /* test station id option */ + if ((p = strstr(rtcm->opt, "-STA=")) && sscanf(p, "-STA=%d", &id) == 1) + { + if (staid != id) return 0; + } + /* save station id */ + if (rtcm->staid == 0 || rtcm->obsflag) + { + rtcm->staid = staid; + } + else if (staid != rtcm->staid) + { + type = getbitu(rtcm->buff, 24, 12); + trace(2, "rtcm3 %d staid invalid id=%d %d\n", type, staid, rtcm->staid); + + /* reset station id if station id error */ + rtcm->staid = 0; + return 0; + } + return 1; +} + + +/* decode type 1001-1004 message header --------------------------------------*/ +int decode_head1001(rtcm_t *rtcm, int *sync) +{ + double tow; + char *msg; + int i = 24, staid, nsat, type; + + type = getbitu(rtcm->buff, i, 12); i += 12; + + if (i+52 <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + tow = getbitu(rtcm->buff, i, 30)*0.001; i += 30; + *sync = getbitu(rtcm->buff, i, 1); i += 1; + nsat = getbitu(rtcm->buff, i, 5); + } + else + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + adjweek(rtcm, tow); + + trace(4, "decode_head1001: time=%s nsat=%d sync=%d\n", time_str(rtcm->time, 2), + nsat, *sync); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d %s nsat=%2d sync=%d", staid, + time_str(rtcm->time, 2), nsat, *sync); + } + return nsat; +} + + +/* decode type 1001: L1-only gps rtk observation -----------------------------*/ +int decode_type1001(rtcm_t *rtcm) +{ + int sync; + if (decode_head1001(rtcm, &sync) < 0) return -1; + rtcm->obsflag=!sync; + return sync?0:1; +} + + +/* decode type 1002: extended L1-only gps rtk observables --------------------*/ +int decode_type1002(rtcm_t *rtcm) +{ + double pr1, cnr1, tt, cp1; + int i = 24+64, j, index, nsat, sync, prn, code, sat, ppr1, lock1, amb, sys; + + if ((nsat = decode_head1001(rtcm, &sync)) < 0) return -1; + + for (j = 0;j < nsat && rtcm->obs.n < MAXOBS && i+74 <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + code = getbitu(rtcm->buff, i, 1); i += 1; + pr1 = getbitu(rtcm->buff, i, 24); i += 24; + ppr1 = getbits(rtcm->buff, i, 20); i += 20; + lock1 = getbitu(rtcm->buff, i, 7); i += 7; + amb = getbitu(rtcm->buff, i, 8); i += 8; + cnr1 = getbitu(rtcm->buff, i, 8); i += 8; + if (prn < 40) + { + sys = SYS_GPS; + } + else + { + sys = SYS_SBS; prn += 80; + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1002 satellite number error: prn=%d\n", prn); + continue; + } + tt = timediff(rtcm->obs.data[0].time, rtcm->time); + if (rtcm->obsflag || fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs, rtcm->time, sat)) < 0) continue; + pr1 = pr1*0.02+amb*PRUNIT_GPS; + if (ppr1 != (int)0xFFF80000) + { + rtcm->obs.data[index].P[0] = pr1; + cp1 = adjcp(rtcm, sat, 0, ppr1*0.0005/lam_carr[0]); + rtcm->obs.data[index].L[0] = pr1/lam_carr[0]+cp1; + } + rtcm->obs.data[index].LLI[0] = lossoflock(rtcm, sat, 0, lock1); + rtcm->obs.data[index].SNR[0] = snratio(cnr1*0.25); + rtcm->obs.data[index].code[0] = code?CODE_L1P:CODE_L1C; + } + return sync?0:1; +} + + +/* decode type 1003: L1&L2 gps rtk observables -------------------------------*/ +int decode_type1003(rtcm_t *rtcm) +{ + int sync; + if (decode_head1001(rtcm, &sync) < 0) return -1; + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 1004: extended L1&L2 gps rtk observables ----------------------*/ +int decode_type1004(rtcm_t *rtcm) +{ + const int L2codes[] = {CODE_L2X, CODE_L2P, CODE_L2D, CODE_L2W}; + double pr1, cnr1, cnr2, tt, cp1, cp2; + int i = 24+64, j, index, nsat, sync, prn, sat, code1, code2, pr21, ppr1, ppr2; + int lock1, lock2, amb, sys; + + if ((nsat = decode_head1001(rtcm, &sync)) < 0) return -1; + + for (j = 0;j < nsat && rtcm->obs.n < MAXOBS && i+125 <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + code1 = getbitu(rtcm->buff, i, 1); i += 1; + pr1 = getbitu(rtcm->buff, i, 24); i += 24; + ppr1 = getbits(rtcm->buff, i, 20); i += 20; + lock1 = getbitu(rtcm->buff, i, 7); i += 7; + amb = getbitu(rtcm->buff, i, 8); i += 8; + cnr1 = getbitu(rtcm->buff, i, 8); i += 8; + code2 = getbitu(rtcm->buff, i, 2); i += 2; + pr21 = getbits(rtcm->buff, i, 14); i += 14; + ppr2 = getbits(rtcm->buff, i, 20); i += 20; + lock2 = getbitu(rtcm->buff, i, 7); i += 7; + cnr2 = getbitu(rtcm->buff, i, 8); i += 8; + if (prn < 40) + { + sys = SYS_GPS; + } + else + { + sys = SYS_SBS; prn += 80; + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1004 satellite number error: sys=%d prn=%d\n", sys, prn); + continue; + } + tt = timediff(rtcm->obs.data[0].time, rtcm->time); + if (rtcm->obsflag || fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs, rtcm->time, sat)) < 0) continue; + pr1 = pr1*0.02+amb*PRUNIT_GPS; + if (ppr1 != (int)0xFFF80000) + { + rtcm->obs.data[index].P[0] = pr1; + cp1 = adjcp(rtcm, sat, 0, ppr1*0.0005/lam_carr[0]); + rtcm->obs.data[index].L[0] = pr1/lam_carr[0]+cp1; + } + rtcm->obs.data[index].LLI[0] = lossoflock(rtcm, sat, 0, lock1); + rtcm->obs.data[index].SNR[0] = snratio(cnr1*0.25); + rtcm->obs.data[index].code[0] = code1?CODE_L1P:CODE_L1C; + + if (pr21 != (int)0xFFFFE000) + { + rtcm->obs.data[index].P[1] = pr1+pr21*0.02; + } + if (ppr2 != (int)0xFFF80000) + { + cp2 = adjcp(rtcm, sat, 1, ppr2*0.0005/lam_carr[1]); + rtcm->obs.data[index].L[1] = pr1/lam_carr[1]+cp2; + } + rtcm->obs.data[index].LLI[1] = lossoflock(rtcm, sat, 1, lock2); + rtcm->obs.data[index].SNR[1] = snratio(cnr2*0.25); + rtcm->obs.data[index].code[1] = L2codes[code2]; + } + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* get signed 38bit field ----------------------------------------------------*/ +double getbits_38(const unsigned char *buff, int pos) +{ + return (double)getbits(buff, pos, 32)*64.0+getbitu(buff, pos+32, 6); +} + + +/* decode type 1005: stationary rtk reference station arp --------------------*/ +int decode_type1005(rtcm_t *rtcm) +{ + double rr[3], re[3], pos[3]; + char *msg; + int i = 24+12, j, staid, itrf; + + if (i+140 == rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + itrf = getbitu(rtcm->buff, i, 6); i += 6+4; + rr[0] = getbits_38(rtcm->buff, i); i += 38+2; + rr[1] = getbits_38(rtcm->buff, i); i += 38+2; + rr[2] = getbits_38(rtcm->buff, i); + } + else + { + trace(2, "rtcm3 1005 length error: len=%d\n", rtcm->len); + return -1; + } + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + for (j = 0;j < 3;j++) re[j] = rr[j]*0.0001; + ecef2pos(re, pos); + sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f", staid, pos[0]*R2D, pos[1]*R2D, + pos[2]); + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + rtcm->sta.deltype = 0; /* xyz */ + for (j = 0;j < 3;j++) + { + rtcm->sta.pos[j] = rr[j]*0.0001; + rtcm->sta.del[j] = 0.0; + } + rtcm->sta.hgt = 0.0; + rtcm->sta.itrf = itrf; + return 5; +} + + +/* decode type 1006: stationary rtk reference station arp with height --------*/ +int decode_type1006(rtcm_t *rtcm) +{ + double rr[3], re[3], pos[3], anth; + char *msg; + int i = 24+12, j, staid, itrf; + + if (i+156 <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + itrf = getbitu(rtcm->buff, i, 6); i += 6+4; + rr[0] = getbits_38(rtcm->buff, i); i += 38+2; + rr[1] = getbits_38(rtcm->buff, i); i += 38+2; + rr[2] = getbits_38(rtcm->buff, i); i += 38; + anth = getbitu(rtcm->buff, i, 16); + } + else + { + trace(2, "rtcm3 1006 length error: len=%d\n", rtcm->len); + return -1; + } + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + for (j = 0;j < 3;j++) re[j] = rr[j]*0.0001; + ecef2pos(re, pos); + sprintf(msg, " staid=%4d pos=%.8f %.8f %.3f anth=%.3f", staid, pos[0]*R2D, + pos[1]*R2D, pos[2], anth); + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + rtcm->sta.deltype = 1; /* xyz */ + for (j = 0;j < 3;j++) + { + rtcm->sta.pos[j] = rr[j]*0.0001; + rtcm->sta.del[j] = 0.0; + } + rtcm->sta.hgt = anth*0.0001; + rtcm->sta.itrf = itrf; + return 5; +} + + +/* decode type 1007: antenna descriptor --------------------------------------*/ +int decode_type1007(rtcm_t *rtcm) +{ + char des[32] = ""; + char *msg; + int i = 24+12, j, staid, n, setup; + + n = getbitu(rtcm->buff, i+12, 8); + + if (i+28+8*n <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12+8; + for (j = 0;j < n && j < 31;j++) + { + des[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + setup = getbitu(rtcm->buff, i, 8); + } + else + { + trace(2, "rtcm3 1007 length error: len=%d\n", rtcm->len); + return -1; + } + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d", staid); + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + strncpy(rtcm->sta.antdes, des, n); rtcm->sta.antdes[n] = '\0'; + rtcm->sta.antsetup = setup; + rtcm->sta.antsno[0] = '\0'; + return 5; +} + + +/* decode type 1008: antenna descriptor & serial number ----------------------*/ +int decode_type1008(rtcm_t *rtcm) +{ + char des[32] = "", sno[32] = ""; + char *msg; + int i = 24+12, j, staid, n, m, setup; + + n = getbitu(rtcm->buff, i+12, 8); + m = getbitu(rtcm->buff, i+28+8*n, 8); + + if (i+36+8*(n+m) <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12+8; + for (j = 0;j < n && j < 31;j++) + { + des[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + setup = getbitu(rtcm->buff, i, 8); i += 8+8; + for (j = 0;j < m && j < 31;j++) + { + sno[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + } + else + { + trace(2, "rtcm3 1008 length error: len=%d\n", rtcm->len); + return -1; + } + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d", staid); + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + strncpy(rtcm->sta.antdes, des, n); rtcm->sta.antdes[n] = '\0'; + rtcm->sta.antsetup = setup; + strncpy(rtcm->sta.antsno, sno, m); rtcm->sta.antsno[m] = '\0'; + return 5; +} + + +/* decode type 1009-1012 message header --------------------------------------*/ +int decode_head1009(rtcm_t *rtcm, int *sync) +{ + double tod; + char *msg; + int i = 24, staid, nsat, type; + + type = getbitu(rtcm->buff, i, 12); i += 12; + + if (i+49 <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + tod = getbitu(rtcm->buff, i, 27)*0.001; i += 27; /* sec in a day */ + *sync = getbitu(rtcm->buff, i, 1); i += 1; + nsat = getbitu(rtcm->buff, i, 5); + } + else + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + adjday_glot(rtcm, tod); + + trace(4, "decode_head1009: time=%s nsat=%d sync=%d\n", time_str(rtcm->time, 2), + nsat, *sync); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d %s nsat=%2d sync=%d", staid, + time_str(rtcm->time, 2), nsat, *sync); + } + return nsat; +} + + +/* decode type 1009: L1-only glonass rtk observables -------------------------*/ +int decode_type1009(rtcm_t *rtcm) +{ + int sync; + if (decode_head1009(rtcm, &sync) < 0) return -1; + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 1010: extended L1-only glonass rtk observables ----------------*/ +int decode_type1010(rtcm_t *rtcm) +{ + double pr1, cnr1, tt, cp1, lam1; + int i = 24+61, j, index, nsat, sync, prn, sat, code, freq, ppr1, lock1, amb, sys = SYS_GLO; + + if ((nsat = decode_head1009(rtcm, &sync)) < 0) return -1; + + for (j = 0;j < nsat && rtcm->obs.n < MAXOBS && i+79 <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + code = getbitu(rtcm->buff, i, 1); i += 1; + freq = getbitu(rtcm->buff, i, 5); i += 5; + pr1 = getbitu(rtcm->buff, i, 25); i += 25; + ppr1 = getbits(rtcm->buff, i, 20); i += 20; + lock1 = getbitu(rtcm->buff, i, 7); i += 7; + amb = getbitu(rtcm->buff, i, 7); i += 7; + cnr1 = getbitu(rtcm->buff, i, 8); i += 8; + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1010 satellite number error: prn=%d\n", prn); + continue; + } + tt = timediff(rtcm->obs.data[0].time, rtcm->time); + if (rtcm->obsflag || fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs, rtcm->time, sat)) < 0) continue; + pr1 = pr1*0.02+amb*PRUNIT_GLO; + if (ppr1 != (int)0xFFF80000) + { + rtcm->obs.data[index].P[0] = pr1; + lam1 = SPEED_OF_LIGHT/(FREQ1_GLO+DFRQ1_GLO*(freq-7)); + cp1 = adjcp(rtcm, sat, 0, ppr1*0.0005/lam1); + rtcm->obs.data[index].L[0] = pr1/lam1+cp1; + } + rtcm->obs.data[index].LLI[0] = lossoflock(rtcm, sat, 0, lock1); + rtcm->obs.data[index].SNR[0] = snratio(cnr1*0.25); + rtcm->obs.data[index].code[0] = code?CODE_L1P:CODE_L1C; + } + return sync?0:1; +} + + +/* decode type 1011: L1&L2 glonass rtk observables ---------------------------*/ +int decode_type1011(rtcm_t *rtcm) +{ + int sync; + if (decode_head1009(rtcm, &sync) < 0) return -1; + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 1012: extended L1&L2 glonass rtk observables ------------------*/ +int decode_type1012(rtcm_t *rtcm) +{ + double pr1, cnr1, cnr2, tt, cp1, cp2, lam1, lam2; + int i = 24+61, j, index, nsat, sync, prn, sat, freq, code1, code2, pr21, ppr1, ppr2; + int lock1, lock2, amb, sys = SYS_GLO; + + if ((nsat = decode_head1009(rtcm, &sync)) < 0) return -1; + + for (j = 0;j < nsat && rtcm->obs.n < MAXOBS && i+130 <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + code1 = getbitu(rtcm->buff, i, 1); i += 1; + freq = getbitu(rtcm->buff, i, 5); i += 5; + pr1 = getbitu(rtcm->buff, i, 25); i += 25; + ppr1 = getbits(rtcm->buff, i, 20); i += 20; + lock1 = getbitu(rtcm->buff, i, 7); i += 7; + amb = getbitu(rtcm->buff, i, 7); i += 7; + cnr1 = getbitu(rtcm->buff, i, 8); i += 8; + code2 = getbitu(rtcm->buff, i, 2); i += 2; + pr21 = getbits(rtcm->buff, i, 14); i += 14; + ppr2 = getbits(rtcm->buff, i, 20); i += 20; + lock2 = getbitu(rtcm->buff, i, 7); i += 7; + cnr2 = getbitu(rtcm->buff, i, 8); i += 8; + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1012 satellite number error: sys=%d prn=%d\n", sys, prn); + continue; + } + tt = timediff(rtcm->obs.data[0].time, rtcm->time); + if (rtcm->obsflag || fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + if ((index = obsindex(&rtcm->obs, rtcm->time, sat)) < 0) continue; + pr1 = pr1*0.02+amb*PRUNIT_GLO; + if (ppr1 != (int)0xFFF80000) + { + lam1 = SPEED_OF_LIGHT/(FREQ1_GLO+DFRQ1_GLO*(freq-7)); + rtcm->obs.data[index].P[0] = pr1; + cp1 = adjcp(rtcm, sat, 0, ppr1*0.0005/lam1); + rtcm->obs.data[index].L[0] = pr1/lam1+cp1; + } + rtcm->obs.data[index].LLI[0] = lossoflock(rtcm, sat, 0, lock1); + rtcm->obs.data[index].SNR[0] = snratio(cnr1*0.25); + rtcm->obs.data[index].code[0] = code1?CODE_L1P:CODE_L1C; + + if (pr21 != (int)0xFFFFE000) + { + rtcm->obs.data[index].P[1] = pr1+pr21*0.02; + } + if (ppr2 != (int)0xFFF80000) + { + lam2 = SPEED_OF_LIGHT/(FREQ2_GLO+DFRQ2_GLO*(freq-7)); + cp2 = adjcp(rtcm, sat, 1, ppr2*0.0005/lam2); + rtcm->obs.data[index].L[1] = pr1/lam2+cp2; + } + rtcm->obs.data[index].LLI[1] = lossoflock(rtcm, sat, 1, lock2); + rtcm->obs.data[index].SNR[1] = snratio(cnr2*0.25); + rtcm->obs.data[index].code[1] = code2?CODE_L2P:CODE_L2C; + } + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 1013: system parameters ---------------------------------------*/ +int decode_type1013(rtcm_t *rtcm) +{ + return 0; +} + + +/* decode type 1019: gps ephemerides -----------------------------------------*/ +int decode_type1019(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, sys = SYS_GPS; + + if (i+476 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + week = getbitu(rtcm->buff, i, 10); i += 10; + eph.sva = getbitu(rtcm->buff, i, 4); i += 4; + eph.code = getbitu(rtcm->buff, i, 2); i += 2; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + eph.iode = getbitu(rtcm->buff, i, 8); i += 8; + toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8; + eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16; + eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22; + eph.iodc = getbitu(rtcm->buff, i, 10); i += 10; + eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8; + eph.svh = getbitu(rtcm->buff, i, 6); i += 6; + eph.flag = getbitu(rtcm->buff, i, 1); i += 1; + eph.fit = getbitu(rtcm->buff, i, 1)?0.0:4.0; /* 0:4hr, 1:>4hr */ + } + else + { + trace(2, "rtcm3 1019 length error: len=%d\n", rtcm->len); + return -1; + } + if (prn >= 40) + { + sys = SYS_SBS; prn += 80; + } + trace(4, "decode_type1019: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X", + prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1019 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = adjgpsweek(week); + eph.toe = gpst2time(eph.week, eph.toes); + eph.toc = gpst2time(eph.week, toc); + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (eph.iode == rtcm->nav.eph[sat-1].iode) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 1020: glonass ephemerides -------------------------------------*/ +int decode_type1020(rtcm_t *rtcm) +{ + geph_t geph = {0}; + double tk_h, tk_m, tk_s, toe, tow, tod, tof; + char *msg; + int i = 24+12, prn, sat, week, tb, bn, sys = SYS_GLO; + + if (i+348 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + geph.frq = getbitu(rtcm->buff, i, 5)-7; i += 5+2+2; + tk_h = getbitu(rtcm->buff, i, 5); i += 5; + tk_m = getbitu(rtcm->buff, i, 6); i += 6; + tk_s = getbitu(rtcm->buff, i, 1)*30.0; i += 1; + bn = getbitu(rtcm->buff, i, 1); i += 1+1; + tb = getbitu(rtcm->buff, i, 7); i += 7; + geph.vel[0] = getbitg(rtcm->buff, i, 24)*TWO_N20*1E3; i += 24; + geph.pos[0] = getbitg(rtcm->buff, i, 27)*TWO_N11*1E3; i += 27; + geph.acc[0] = getbitg(rtcm->buff, i, 5)*TWO_N30*1E3; i += 5; + geph.vel[1] = getbitg(rtcm->buff, i, 24)*TWO_N20*1E3; i += 24; + geph.pos[1] = getbitg(rtcm->buff, i, 27)*TWO_N11*1E3; i += 27; + geph.acc[1] = getbitg(rtcm->buff, i, 5)*TWO_N30*1E3; i += 5; + geph.vel[2] = getbitg(rtcm->buff, i, 24)*TWO_N20*1E3; i += 24; + geph.pos[2] = getbitg(rtcm->buff, i, 27)*TWO_N11*1E3; i += 27; + geph.acc[2] = getbitg(rtcm->buff, i, 5)*TWO_N30*1E3; i += 5+1; + geph.gamn = getbitg(rtcm->buff, i, 11)*TWO_N40; i += 11+3; + geph.taun = getbitg(rtcm->buff, i, 22)*TWO_N30; + } + else + { + trace(2, "rtcm3 1020 length error: len=%d\n", rtcm->len); + return -1; + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1020 satellite number error: prn=%d\n", prn); + return -1; + } + trace(4, "decode_type1020: prn=%d tk=%02.0f:%02.0f:%02.0f\n", prn, tk_h, tk_m, tk_s); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d tk=%02.0f:%02.0f:%02.0f frq=%2d bn=%d tb=%d", + prn, tk_h, tk_m, tk_s, geph.frq, bn, tb); + } + geph.sat = sat; + geph.svh = bn; + geph.iode = tb&0x7F; + if (rtcm->time.time == 0) rtcm->time = utc2gpst(timeget()); + tow = time2gpst(gpst2utc(rtcm->time), &week); + tod = fmod(tow, 86400.0); tow -= tod; + tof = tk_h*3600.0+tk_m*60.0+tk_s-10800.0; /* lt->utc */ + if (tof < tod-43200.0) tof += 86400.0; + else if (tof>tod+43200.0) tof -= 86400.0; + geph.tof = utc2gpst(gpst2time(week, tow+tof)); + toe = tb*900.0-10800.0; /* lt->utc */ + if (toe < tod-43200.0) toe += 86400.0; + else if (toe>tod+43200.0) toe -= 86400.0; + geph.toe = utc2gpst(gpst2time(week, tow+toe)); /* utc->gpst */ + + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (fabs(timediff(geph.toe, rtcm->nav.geph[prn-1].toe)) < 1.0 && + geph.svh == rtcm->nav.geph[prn-1].svh) return 0; /* unchanged */ + } + rtcm->nav.geph[prn-1] = geph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 1021: helmert/abridged molodenski -----------------------------*/ +int decode_type1021(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1021: not supported message\n"); + return 0; +} + + +/* decode type 1022: moledenski-badekas transfromation -----------------------*/ +int decode_type1022(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1022: not supported message\n"); + return 0; +} + + +/* decode type 1023: residual, ellipoidal grid representation ----------------*/ +int decode_type1023(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1023: not supported message\n"); + return 0; +} + + +/* decode type 1024: residual, plane grid representation ---------------------*/ +int decode_type1024(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1024: not supported message\n"); + return 0; +} + + +/* decode type 1025: projection (types except LCC2SP, OM) ---------------------*/ +int decode_type1025(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1025: not supported message\n"); + return 0; +} + + +/* decode type 1026: projection (LCC2SP - lambert conic conformal (2sp)) -----*/ +int decode_type1026(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1026: not supported message\n"); + return 0; +} + + +/* decode type 1027: projection (type OM - oblique mercator) -----------------*/ +int decode_type1027(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1027: not supported message\n"); + return 0; +} + + +/* decode type 1029: unicode text string -------------------------------------*/ +int decode_type1029(rtcm_t *rtcm) +{ + char *msg; + int i = 24+12, j, staid, mjd, tod, nchar, cunit; + + if (i+60 <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + mjd = getbitu(rtcm->buff, i, 16); i += 16; + tod = getbitu(rtcm->buff, i, 17); i += 17; + nchar = getbitu(rtcm->buff, i, 7); i += 7; + cunit = getbitu(rtcm->buff, i, 8); i += 8; + } + else + { + trace(2, "rtcm3 1029 length error: len=%d\n", rtcm->len); + return -1; + } + if (i+nchar*8>rtcm->len*8) + { + trace(2, "rtcm3 1029 length error: len=%d nchar=%d\n", rtcm->len, nchar); + return -1; + } + for (j = 0;j < nchar && j < 126;j++) + { + rtcm->msg[j] = getbitu(rtcm->buff, i, 8); i += 8; + } + rtcm->msg[j] = '\0'; + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d text=%s", staid, rtcm->msg); + } + return 0; +} + + +/* decode type 1030: network rtk residual ------------------------------------*/ +int decode_type1030(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1030: not supported message\n"); + return 0; +} + + +/* decode type 1031: glonass network rtk residual ----------------------------*/ +int decode_type1031(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1031: not supported message\n"); + return 0; +} + + +/* decode type 1032: physical reference station position information ---------*/ +int decode_type1032(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1032: not supported message\n"); + return 0; +} + + +/* decode type 1033: receiver and antenna descriptor -------------------------*/ +int decode_type1033(rtcm_t *rtcm) +{ + char des[32] = "", sno[32] = "", rec[32] = "", ver[32] = "", rsn[32] = ""; + char *msg; + int i = 24+12, j, staid, n, m, n1, n2, n3, setup; + + n = getbitu(rtcm->buff, i+12, 8); + m = getbitu(rtcm->buff, i+28+8*n, 8); + n1 = getbitu(rtcm->buff, i+36+8*(n+m), 8); + n2 = getbitu(rtcm->buff, i+44+8*(n+m+n1), 8); + n3 = getbitu(rtcm->buff, i+52+8*(n+m+n1+n2), 8); + + if (i+60+8*(n+m+n1+n2+n3) <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12+8; + for (j = 0;j < n && j < 31;j++) + { + des[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + setup = getbitu(rtcm->buff, i, 8); i += 8+8; + for (j = 0;j < m && j < 31;j++) + { + sno[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + i += 8; + for (j = 0;j < n1 && j < 31;j++) + { + rec[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + i += 8; + for (j = 0;j < n2 && j < 31;j++) + { + ver[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + i += 8; + for (j = 0;j < n3 && j < 31;j++) + { + rsn[j] = (char)getbitu(rtcm->buff, i, 8); i += 8; + } + } + else + { + trace(2, "rtcm3 1033 length error: len=%d\n", rtcm->len); + return -1; + } + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d", staid); + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + strncpy(rtcm->sta.antdes, des, n ); rtcm->sta.antdes [n] = '\0'; + rtcm->sta.antsetup = setup; + strncpy(rtcm->sta.antsno, sno, m ); rtcm->sta.antsno [m] = '\0'; + strncpy(rtcm->sta.rectype, rec, n1); rtcm->sta.rectype[n1] = '\0'; + strncpy(rtcm->sta.recver, ver, n2); rtcm->sta.recver [n2] = '\0'; + strncpy(rtcm->sta.recsno, rsn, n3); rtcm->sta.recsno [n3] = '\0'; + + trace(3, "rtcm3 1033: ant=%s:%s rec=%s:%s:%s\n", des, sno, rec, ver, rsn); + return 5; +} + + +/* decode type 1034: gps network fkp gradient --------------------------------*/ +int decode_type1034(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1034: not supported message\n"); + return 0; +} + + +/* decode type 1035: glonass network fkp gradient ----------------------------*/ +int decode_type1035(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1035: not supported message\n"); + return 0; +} + + +/* decode type 1037: glonass network rtk ionospheric correction difference ---*/ +int decode_type1037(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1037: not supported message\n"); + return 0; +} + + +/* decode type 1038: glonass network rtk geometic correction difference ------*/ +int decode_type1038(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1038: not supported message\n"); + return 0; +} + + +/* decode type 1039: glonass network rtk combined correction difference ------*/ +int decode_type1039(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1039: not supported message\n"); + return 0; +} + + +/* decode type 1044: qzss ephemerides (ref [15]) -----------------------------*/ +int decode_type1044(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, sys = SYS_QZS; + + if (i+473 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 4)+192; i += 4; + toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8; + eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16; + eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22; + eph.iode = getbitu(rtcm->buff, i, 8); i += 8; + eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + eph.code = getbitu(rtcm->buff, i, 2); i += 2; + week = getbitu(rtcm->buff, i, 10); i += 10; + eph.sva = getbitu(rtcm->buff, i, 4); i += 4; + eph.svh = getbitu(rtcm->buff, i, 6); i += 6; + eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8; + eph.iodc = getbitu(rtcm->buff, i, 10); i += 10; + eph.fit = getbitu(rtcm->buff, i, 1)?0.0:2.0; /* 0:2hr, 1:>2hr */ + } + else + { + trace(2, "rtcm3 1044 length error: len=%d\n", rtcm->len); + return -1; + } + trace(4, "decode_type1044: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%3d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X", + prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1044 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = adjgpsweek(week); + eph.toe = gpst2time(eph.week, eph.toes); + eph.toc = gpst2time(eph.week, toc); + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (eph.iode == rtcm->nav.eph[sat-1].iode && + eph.iodc == rtcm->nav.eph[sat-1].iodc) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 1045: galileo satellite ephemerides (ref [15]) ----------------*/ +int decode_type1045(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, e5a_hs, e5a_dvs, rsv, sys = SYS_GAL; + + if (i+484 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + week = getbitu(rtcm->buff, i, 12); i += 12; /* gst-week */ + eph.iode = getbitu(rtcm->buff, i, 10); i += 10; + eph.sva = getbitu(rtcm->buff, i, 8); i += 8; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + toc = getbitu(rtcm->buff, i, 14)*60.0; i += 14; + eph.f2 = getbits(rtcm->buff, i, 6)*TWO_N59; i += 6; + eph.f1 = getbits(rtcm->buff, i, 21)*TWO_N46; i += 21; + eph.f0 = getbits(rtcm->buff, i, 31)*TWO_N34; i += 31; + eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 14)*60.0; i += 14; + eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.tgd[0] = getbits(rtcm->buff, i, 10)*TWO_N32; i += 10; /* E5a/E1 */ + e5a_hs = getbitu(rtcm->buff, i, 2); i += 2; /* OSHS */ + e5a_dvs = getbitu(rtcm->buff, i, 1); i += 1; /* OSDVS */ + rsv = getbitu(rtcm->buff, i, 7); + } + else + { + trace(2, "rtcm3 1045 length error: len=%d\n", rtcm->len); + return -1; + } + trace(4, "decode_type1045: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d iode=%3d week=%d toe=%6.0f toc=%6.0f hs=%d dvs=%d", + prn, eph.iode, week, eph.toes, toc, e5a_hs, e5a_dvs); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1045 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = week+1024; /* gal-week = gst-week + 1024 */ + eph.toe = gpst2time(eph.week, eph.toes); + eph.toc = gpst2time(eph.week, toc); + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + eph.svh = (e5a_hs<<4)+(e5a_dvs<<3); + eph.code = 2; /* data source = f/nav e5a */ + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (eph.iode == rtcm->nav.eph[sat-1].iode) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 1046: galileo satellite ephemerides (extension for IGS MGEX) --*/ +int decode_type1046(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, e5a_hs, e5a_dvs, rsv, sys = SYS_GAL; + + if (i+484 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + week = getbitu(rtcm->buff, i, 12); i += 12; /* gst-week */ + eph.iode = getbitu(rtcm->buff, i, 10); i += 10; + eph.sva = getbitu(rtcm->buff, i, 8); i += 8; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + toc = getbitu(rtcm->buff, i, 14)*60.0; i += 14; + eph.f2 = getbits(rtcm->buff, i, 6)*TWO_N59; i += 6; + eph.f1 = getbits(rtcm->buff, i, 21)*TWO_N46; i += 21; + eph.f0 = getbits(rtcm->buff, i, 31)*TWO_N34; i += 31; + eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 14)*60.0; i += 14; + eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.tgd[0] = getbits(rtcm->buff, i, 10)*TWO_N32; i += 10; /* E5a/E1 */ + e5a_hs = getbitu(rtcm->buff, i, 2); i += 2; /* OSHS */ + e5a_dvs = getbitu(rtcm->buff, i, 1); i += 1; /* OSDVS */ + rsv = getbitu(rtcm->buff, i, 7); + } + else + { + trace(2, "rtcm3 1046 length error: len=%d\n", rtcm->len); + return -1; + } + trace(4, "decode_type1046: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d iode=%3d week=%d toe=%6.0f toc=%6.0f hs=%d dvs=%d", + prn, eph.iode, week, eph.toes, toc, e5a_hs, e5a_dvs); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1046 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = week+1024; /* gal-week = gst-week + 1024 */ + eph.toe = gpst2time(eph.week, eph.toes); + eph.toc = gpst2time(eph.week, toc); + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + eph.svh = (e5a_hs<<4)+(e5a_dvs<<3); + eph.code = 2; /* data source = f/nav e5a */ + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (eph.iode == rtcm->nav.eph[sat-1].iode) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 1047: beidou ephemerides (tentative mt and format) ------------*/ +int decode_type1047(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, sys = SYS_BDS; + + if (i+476 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + week = getbitu(rtcm->buff, i, 10); i += 10; + eph.sva = getbitu(rtcm->buff, i, 4); i += 4; + eph.code = getbitu(rtcm->buff, i, 2); i += 2; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + eph.iode = getbitu(rtcm->buff, i, 8); i += 8; + toc = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.f2 = getbits(rtcm->buff, i, 8)*TWO_N55; i += 8; + eph.f1 = getbits(rtcm->buff, i, 16)*TWO_N43; i += 16; + eph.f0 = getbits(rtcm->buff, i, 22)*TWO_N31; i += 22; + eph.iodc = getbitu(rtcm->buff, i, 10); i += 10; + eph.crs = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 16)*16.0; i += 16; + eph.cic = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 16)*TWO_N29; i += 16; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 16)*TWO_N5; i += 16; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.tgd[0] = getbits(rtcm->buff, i, 8)*TWO_N31; i += 8; + eph.svh = getbitu(rtcm->buff, i, 6); i += 6; + eph.flag = getbitu(rtcm->buff, i, 1); i += 1; + eph.fit = getbitu(rtcm->buff, i, 1)?0.0:4.0; /* 0:4hr, 1:>4hr */ + } + else + { + trace(2, "rtcm3 1047 length error: len=%d\n", rtcm->len); + return -1; + } + trace(4, "decode_type1047: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X", + prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 1047 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = adjbdtweek(week); + eph.toe = bdt2gpst(bdt2time(eph.week, eph.toes)); /* bdt -> gpst */ + eph.toc = bdt2gpst(bdt2time(eph.week, toc)); /* bdt -> gpst */ + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + if (!strstr(rtcm->opt, "-EPHALL")) + + { + if (timediff(eph.toe, rtcm->nav.eph[sat-1].toe) == 0.0 && + eph.iode == rtcm->nav.eph[sat-1].iode && + eph.iodc == rtcm->nav.eph[sat-1].iodc) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode type 63: beidou ephemerides (rtcm draft) ---------------------------*/ +int decode_type63(rtcm_t *rtcm) +{ + eph_t eph = {0}; + double toc, sqrtA; + char *msg; + int i = 24+12, prn, sat, week, sys = SYS_BDS; + + if (i+499 <= rtcm->len*8) + { + prn = getbitu(rtcm->buff, i, 6); i += 6; + week = getbitu(rtcm->buff, i, 13); i += 13; + eph.sva = getbitu(rtcm->buff, i, 4); i += 4; + eph.idot = getbits(rtcm->buff, i, 14)*TWO_N43*SC2RAD; i += 14; + eph.iode = getbitu(rtcm->buff, i, 5); i += 5; /* AODE */ + toc = getbitu(rtcm->buff, i, 17)*8.0; i += 17; + eph.f2 = getbits(rtcm->buff, i, 11)*TWO_N55; i += 11; + eph.f1 = getbits(rtcm->buff, i, 22)*TWO_N50; i += 22; + eph.f0 = getbits(rtcm->buff, i, 24)*TWO_N33; i += 24; + eph.iodc = getbitu(rtcm->buff, i, 5); i += 5; /* AODC */ + eph.crs = getbits(rtcm->buff, i, 18)*TWO_N6; i += 18; + eph.deln = getbits(rtcm->buff, i, 16)*TWO_N43*SC2RAD; i += 16; + eph.M0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cuc = getbits(rtcm->buff, i, 18)*TWO_N31; i += 18; + eph.e = getbitu(rtcm->buff, i, 32)*TWO_N33; i += 32; + eph.cus = getbits(rtcm->buff, i, 18)*TWO_N31; i += 18; + sqrtA = getbitu(rtcm->buff, i, 32)*TWO_N19; i += 32; + eph.toes = getbitu(rtcm->buff, i, 17)*8.0; i += 17; + eph.cic = getbits(rtcm->buff, i, 18)*TWO_N31; i += 18; + eph.OMG0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.cis = getbits(rtcm->buff, i, 18)*TWO_N31; i += 18; + eph.i0 = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.crc = getbits(rtcm->buff, i, 18)*TWO_N6; i += 18; + eph.omg = getbits(rtcm->buff, i, 32)*TWO_N31*SC2RAD; i += 32; + eph.OMGd = getbits(rtcm->buff, i, 24)*TWO_N43*SC2RAD; i += 24; + eph.tgd[0] = getbits(rtcm->buff, i, 10)*1E-10; i += 10; + eph.tgd[1] = getbits(rtcm->buff, i, 10)*1E-10; i += 10; + eph.svh = getbitu(rtcm->buff, i, 1); i += 1; + } + else + { + trace(2, "rtcm3 63 length error: len=%d\n", rtcm->len); + return -1; + } + trace(4, "decode_type63: prn=%d iode=%d toe=%.0f\n", prn, eph.iode, eph.toes); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " prn=%2d iode=%3d iodc=%3d week=%d toe=%6.0f toc=%6.0f svh=%02X", + prn, eph.iode, eph.iodc, week, eph.toes, toc, eph.svh); + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 63 satellite number error: prn=%d\n", prn); + return -1; + } + eph.sat = sat; + eph.week = adjbdtweek(week); + eph.toe = bdt2gpst(bdt2time(eph.week, eph.toes)); /* bdt -> gpst */ + eph.toc = bdt2gpst(bdt2time(eph.week, toc)); /* bdt -> gpst */ + eph.ttr = rtcm->time; + eph.A = sqrtA*sqrtA; + if (!strstr(rtcm->opt, "-EPHALL")) + { + if (timediff(eph.toe, rtcm->nav.eph[sat-1].toe) == 0.0 && + eph.iode == rtcm->nav.eph[sat-1].iode && + eph.iodc == rtcm->nav.eph[sat-1].iodc) return 0; /* unchanged */ + } + rtcm->nav.eph[sat-1] = eph; + rtcm->ephsat = sat; + return 2; +} + + +/* decode ssr 1, 4 message header ---------------------------------------------*/ +int decode_ssr1_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *refd, int *hsize) +{ + double tod, tow; + char *msg; + int i = 24+12, nsat, udi, provid = 0, solid = 0, ns = 6; + +#ifndef SSR_QZSS_DRAFT_V05 + ns = sys == SYS_QZS?4:6; +#endif + if (i+(sys == SYS_GLO?53:50+ns)>rtcm->len*8) return -1; + + if (sys == SYS_GLO) + { + tod = getbitu(rtcm->buff, i, 17); i += 17; + adjday_glot(rtcm, tod); + } + else + { + tow = getbitu(rtcm->buff, i, 20); i += 20; + adjweek(rtcm, tow); + } + udi = getbitu(rtcm->buff, i, 4); i += 4; + *sync = getbitu(rtcm->buff, i, 1); i += 1; + *refd = getbitu(rtcm->buff, i, 1); i += 1; /* satellite ref datum */ + *iod = getbitu(rtcm->buff, i, 4); i += 4; /* iod */ + provid = getbitu(rtcm->buff, i, 16); i += 16; /* provider id */ + solid = getbitu(rtcm->buff, i, 4); i += 4; /* solution id */ + nsat = getbitu(rtcm->buff, i, ns); i += ns; + *udint = ssrudint[udi]; + + trace(4, "decode_ssr1_head: time=%s sys=%d nsat=%d sync=%d iod=%d provid=%d solid=%d\n", + time_str(rtcm->time, 2), sys, nsat, *sync, *iod, provid, solid); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " %s nsat=%2d iod=%2d udi=%2d sync=%d", + time_str(rtcm->time, 2), nsat, *iod, udi, *sync); + } + *hsize = i; + return nsat; +} + + +/* decode ssr 2, 3, 5, 6 message header -----------------------------------------*/ +int decode_ssr2_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *hsize) +{ + double tod, tow; + char *msg; + int i = 24+12, nsat, udi, provid = 0, solid = 0, ns = 6; + +#ifndef SSR_QZSS_DRAFT_V05 + ns = sys == SYS_QZS?4:6; +#endif + if (i+(sys == SYS_GLO?52:49+ns)>rtcm->len*8) return -1; + + if (sys == SYS_GLO) + { + tod = getbitu(rtcm->buff, i, 17); i += 17; + adjday_glot(rtcm, tod); + } + else + { + tow = getbitu(rtcm->buff, i, 20); i += 20; + adjweek(rtcm, tow); + } + udi = getbitu(rtcm->buff, i, 4); i += 4; + *sync = getbitu(rtcm->buff, i, 1); i += 1; + *iod = getbitu(rtcm->buff, i, 4); i += 4; + provid = getbitu(rtcm->buff, i, 16); i += 16; /* provider id */ + solid = getbitu(rtcm->buff, i, 4); i += 4; /* solution id */ + nsat = getbitu(rtcm->buff, i, ns); i += ns; + *udint = ssrudint[udi]; + + trace(4, "decode_ssr2_head: time=%s sys=%d nsat=%d sync=%d iod=%d provid=%d solid=%d\n", + time_str(rtcm->time, 2), sys, nsat, *sync, *iod, provid, solid); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " %s nsat=%2d iod=%2d udi=%2d sync=%d", + time_str(rtcm->time, 2), nsat, *iod, udi, *sync); + } + *hsize = i; + return nsat; +} + + +/* decode ssr 7 message header -----------------------------------------------*/ +int decode_ssr7_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *dispe, int *mw, int *hsize) +{ + double tod, tow; + char *msg; + int i = 24+12, nsat, udi, provid = 0, solid = 0, ns = 6; + +#ifndef SSR_QZSS_DRAFT_V05 + ns = sys == SYS_QZS?4:6; +#endif + if (i+(sys == SYS_GLO?54:51+ns)>rtcm->len*8) return -1; + + if (sys == SYS_GLO) + { + tod = getbitu(rtcm->buff, i, 17); i += 17; + adjday_glot(rtcm, tod); + } + else + { + tow = getbitu(rtcm->buff, i, 20); i += 20; + adjweek(rtcm, tow); + } + udi = getbitu(rtcm->buff, i, 4); i += 4; + *sync = getbitu(rtcm->buff, i, 1); i += 1; + *iod = getbitu(rtcm->buff, i, 4); i += 4; + provid = getbitu(rtcm->buff, i, 16); i += 16; /* provider id */ + solid = getbitu(rtcm->buff, i, 4); i += 4; /* solution id */ + *dispe = getbitu(rtcm->buff, i, 1); i += 1; /* dispersive bias consistency ind */ + *mw = getbitu(rtcm->buff, i, 1); i += 1; /* MW consistency indicator */ + nsat = getbitu(rtcm->buff, i, ns); i += ns; + *udint = ssrudint[udi]; + + trace(4, "decode_ssr7_head: time=%s sys=%d nsat=%d sync=%d iod=%d provid=%d solid=%d\n", + time_str(rtcm->time, 2), sys, nsat, *sync, *iod, provid, solid); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " %s nsat=%2d iod=%2d udi=%2d sync=%d", + time_str(rtcm->time, 2), nsat, *iod, udi, *sync); + } + *hsize = i; + return nsat; +} + + + + +/* decode ssr 1: orbit corrections -------------------------------------------*/ +int decode_ssr1(rtcm_t *rtcm, int sys) +{ + double udint, deph[3], ddeph[3]; + int i, j, k, type, sync, iod, nsat, prn, sat, iode, iodcrc, refd = 0, np, ni, nj, offp; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr1_head(rtcm, sys, &sync, &iod, &udint, &refd, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; ni = 8; nj = 0; offp = 0; break; + case SYS_GLO: np = 5; ni = 8; nj = 0; offp = 0; break; + case SYS_GAL: np = 6; ni = 10; nj = 0; offp = 0; break; + case SYS_QZS: np = 4; ni = 8; nj = 0; offp = 192; break; + case SYS_BDS: np = 6; ni = 10; nj = 24; offp = 1; break; + case SYS_SBS: np = 6; ni = 9; nj = 24; offp = 120; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+121+np+ni+nj <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + iode = getbitu(rtcm->buff, i, ni); i += ni; + iodcrc = getbitu(rtcm->buff, i, nj); i += nj; + deph [0] = getbits(rtcm->buff, i, 22)*1E-4; i += 22; + deph [1] = getbits(rtcm->buff, i, 20)*4E-4; i += 20; + deph [2] = getbits(rtcm->buff, i, 20)*4E-4; i += 20; + ddeph[0] = getbits(rtcm->buff, i, 21)*1E-6; i += 21; + ddeph[1] = getbits(rtcm->buff, i, 19)*4E-6; i += 19; + ddeph[2] = getbits(rtcm->buff, i, 19)*4E-6; i += 19; + + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [0] = rtcm->time; + rtcm->ssr[sat-1].udi[0] = udint; + rtcm->ssr[sat-1].iod[0] = iod; + rtcm->ssr[sat-1].iode = iode; /* sbas/bds: toe/t0 modulo */ + rtcm->ssr[sat-1].iodcrc = iodcrc; /* sbas/bds: iod crc */ + rtcm->ssr[sat-1].refd = refd; + + for (k = 0;k < 3;k++) + { + rtcm->ssr[sat-1].deph [k] = deph [k]; + rtcm->ssr[sat-1].ddeph[k] = ddeph[k]; + } + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 2: clock corrections -------------------------------------------*/ +int decode_ssr2(rtcm_t *rtcm, int sys) +{ + double udint, dclk[3]; + int i, j, k, type, sync, iod, nsat, prn, sat, np, offp; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr2_head(rtcm, sys, &sync, &iod, &udint, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; offp = 0; break; + case SYS_GLO: np = 5; offp = 0; break; + case SYS_GAL: np = 6; offp = 0; break; + case SYS_QZS: np = 4; offp = 192; break; + case SYS_BDS: np = 6; offp = 1; break; + case SYS_SBS: np = 6; offp = 120; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+70+np <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + dclk[0] = getbits(rtcm->buff, i, 22)*1E-4; i += 22; + dclk[1] = getbits(rtcm->buff, i, 21)*1E-6; i += 21; + dclk[2] = getbits(rtcm->buff, i, 27)*2E-8; i += 27; + + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [1] = rtcm->time; + rtcm->ssr[sat-1].udi[1] = udint; + rtcm->ssr[sat-1].iod[1] = iod; + + for (k = 0;k < 3;k++) + { + rtcm->ssr[sat-1].dclk[k] = dclk[k]; + } + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 3: satellite code biases ---------------------------------------*/ +int decode_ssr3(rtcm_t *rtcm, int sys) +{ + const int *codes; + double udint, bias, cbias[MAXCODE]; + int i, j, k, type, mode, sync, iod, nsat, prn, sat, nbias, np, offp, ncode; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr2_head(rtcm, sys, &sync, &iod, &udint, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; offp = 0; codes = codes_gps; ncode = 17; break; + case SYS_GLO: np = 5; offp = 0; codes = codes_glo; ncode = 4; break; + case SYS_GAL: np = 6; offp = 0; codes = codes_gal; ncode = 19; break; + case SYS_QZS: np = 4; offp = 192; codes = codes_qzs; ncode = 13; break; + case SYS_BDS: np = 6; offp = 1; codes = codes_bds; ncode = 9; break; + case SYS_SBS: np = 6; offp = 120; codes = codes_sbs; ncode = 4; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+5+np <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + nbias = getbitu(rtcm->buff, i, 5); i += 5; + + for (k = 0;k < MAXCODE;k++) cbias[k] = 0.0; + for (k = 0;k < nbias && i+19 <= rtcm->len*8;k++) + { + mode = getbitu(rtcm->buff, i, 5); i += 5; + bias = getbits(rtcm->buff, i, 14)*0.01; i += 14; + if (mode <= ncode) + { + cbias[codes[mode]-1] = (float)bias; + } + else + { + trace(2, "rtcm3 %d not supported mode: mode=%d\n", type, mode); + } + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [4] = rtcm->time; + rtcm->ssr[sat-1].udi[4] = udint; + rtcm->ssr[sat-1].iod[4] = iod; + + for (k = 0;k < MAXCODE;k++) + { + rtcm->ssr[sat-1].cbias[k] = (float)cbias[k]; + } + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 4: combined orbit and clock corrections ------------------------*/ +int decode_ssr4(rtcm_t *rtcm, int sys) +{ + double udint, deph[3], ddeph[3], dclk[3]; + int i, j, k, type, nsat, sync, iod, prn, sat, iode, iodcrc, refd = 0, np, ni, nj, offp; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr1_head(rtcm, sys, &sync, &iod, &udint, &refd, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; ni = 8; nj = 0; offp = 0; break; + case SYS_GLO: np = 5; ni = 8; nj = 0; offp = 0; break; + case SYS_GAL: np = 6; ni = 10; nj = 0; offp = 0; break; + case SYS_QZS: np = 4; ni = 8; nj = 0; offp = 192; break; + case SYS_BDS: np = 6; ni = 10; nj = 24; offp = 1; break; + case SYS_SBS: np = 6; ni = 9; nj = 24; offp = 120; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+191+np+ni+nj <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + iode = getbitu(rtcm->buff, i, ni); i += ni; + iodcrc = getbitu(rtcm->buff, i, nj); i += nj; + deph [0] = getbits(rtcm->buff, i, 22)*1E-4; i += 22; + deph [1] = getbits(rtcm->buff, i, 20)*4E-4; i += 20; + deph [2] = getbits(rtcm->buff, i, 20)*4E-4; i += 20; + ddeph[0] = getbits(rtcm->buff, i, 21)*1E-6; i += 21; + ddeph[1] = getbits(rtcm->buff, i, 19)*4E-6; i += 19; + ddeph[2] = getbits(rtcm->buff, i, 19)*4E-6; i += 19; + + dclk [0] = getbits(rtcm->buff, i, 22)*1E-4; i += 22; + dclk [1] = getbits(rtcm->buff, i, 21)*1E-6; i += 21; + dclk [2] = getbits(rtcm->buff, i, 27)*2E-8; i += 27; + + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [0] = rtcm->ssr[sat-1].t0 [1] = rtcm->time; + rtcm->ssr[sat-1].udi[0] = rtcm->ssr[sat-1].udi[1] = udint; + rtcm->ssr[sat-1].iod[0] = rtcm->ssr[sat-1].iod[1] = iod; + rtcm->ssr[sat-1].iode = iode; + rtcm->ssr[sat-1].iodcrc = iodcrc; + rtcm->ssr[sat-1].refd = refd; + + for (k = 0;k < 3;k++) + { + rtcm->ssr[sat-1].deph [k] = deph [k]; + rtcm->ssr[sat-1].ddeph[k] = ddeph[k]; + rtcm->ssr[sat-1].dclk [k] = dclk [k]; + } + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 5: ura ---------------------------------------------------------*/ +int decode_ssr5(rtcm_t *rtcm, int sys) +{ + double udint; + int i, j, type, nsat, sync, iod, prn, sat, ura, np, offp; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr2_head(rtcm, sys, &sync, &iod, &udint, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; offp = 0; break; + case SYS_GLO: np = 5; offp = 0; break; + case SYS_GAL: np = 6; offp = 0; break; + case SYS_QZS: np = 4; offp = 192; break; + case SYS_BDS: np = 6; offp = 1; break; + case SYS_SBS: np = 6; offp = 120; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+6+np <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + ura = getbitu(rtcm->buff, i, 6); i += 6; + + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [3] = rtcm->time; + rtcm->ssr[sat-1].udi[3] = udint; + rtcm->ssr[sat-1].iod[3] = iod; + rtcm->ssr[sat-1].ura = ura; + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 6: high rate clock correction ----------------------------------*/ +int decode_ssr6(rtcm_t *rtcm, int sys) +{ + double udint, hrclk; + int i, j, type, nsat, sync, iod, prn, sat, np, offp; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr2_head(rtcm, sys, &sync, &iod, &udint, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; offp = 0; break; + case SYS_GLO: np = 5; offp = 0; break; + case SYS_GAL: np = 6; offp = 0; break; + case SYS_QZS: np = 4; offp = 192; break; + case SYS_BDS: np = 6; offp = 1; break; + case SYS_SBS: np = 6; offp = 120; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+22+np <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + hrclk = getbits(rtcm->buff, i, 22)*1E-4; i += 22; + + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [2] = rtcm->time; + rtcm->ssr[sat-1].udi[2] = udint; + rtcm->ssr[sat-1].iod[2] = iod; + rtcm->ssr[sat-1].hrclk = hrclk; + rtcm->ssr[sat-1].update = 1; + } + return sync?0:10; +} + + +/* decode ssr 7: phase bias --------------------------------------------------*/ +int decode_ssr7(rtcm_t *rtcm, int sys) +{ + const int *codes; + double udint, bias, std, pbias[MAXCODE], stdpb[MAXCODE]; + int i, j, k, type, mode, sync, iod, nsat, prn, sat, nbias, ncode, np, mw, offp, sii, swl; + int dispe, sdc, yaw_ang, yaw_rate; + + type = getbitu(rtcm->buff, 24, 12); + + if ((nsat = decode_ssr7_head(rtcm, sys, &sync, &iod, &udint, &dispe, &mw, &i)) < 0) + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + switch (sys) + { + case SYS_GPS: np = 6; offp = 0; codes = codes_gps; ncode = 17; break; + case SYS_GLO: np = 5; offp = 0; codes = codes_glo; ncode = 4; break; + case SYS_GAL: np = 6; offp = 0; codes = codes_gal; ncode = 19; break; + case SYS_QZS: np = 4; offp = 192; codes = codes_qzs; ncode = 13; break; + case SYS_BDS: np = 6; offp = 1; codes = codes_bds; ncode = 9; break; + default: return sync?0:10; + } + for (j = 0;j < nsat && i+5+17+np <= rtcm->len*8;j++) + { + prn = getbitu(rtcm->buff, i, np)+offp; i += np; + nbias = getbitu(rtcm->buff, i, 5); i += 5; + yaw_ang = getbitu(rtcm->buff, i, 9); i += 9; + yaw_rate = getbits(rtcm->buff, i, 8); i += 8; + + for (k = 0;k < MAXCODE;k++) pbias[k] = stdpb[k] = 0.0; + for (k = 0;k < nbias && i+49 <= rtcm->len*8;k++) + { + mode = getbitu(rtcm->buff, i, 5); i += 5; + sii = getbitu(rtcm->buff, i, 1); i += 1; /* integer-indicator */ + swl = getbitu(rtcm->buff, i, 2); i += 2; /* WL integer-indicator */ + sdc = getbitu(rtcm->buff, i, 4); i += 4; /* discontinuity counter */ + bias = getbits(rtcm->buff, i, 20); i += 20; /* phase bias (m) */ + std = getbitu(rtcm->buff, i, 17); i += 17; /* phase bias std-dev (m) */ + if (mode <= ncode) + { + pbias[codes[mode]-1] = bias*0.0001; /* (m) */ + stdpb[codes[mode]-1] = std *0.0001; /* (m) */ + } + else + { + trace(2, "rtcm3 %d not supported mode: mode=%d\n", type, mode); + } + } + if (!(sat = satno(sys, prn))) + { + trace(2, "rtcm3 %d satellite number error: prn=%d\n", type, prn); + continue; + } + rtcm->ssr[sat-1].t0 [5] = rtcm->time; + rtcm->ssr[sat-1].udi[5] = udint; + rtcm->ssr[sat-1].iod[5] = iod; + rtcm->ssr[sat-1].yaw_ang = yaw_ang / 256.0*180.0; /* (deg) */ + rtcm->ssr[sat-1].yaw_rate = yaw_rate / 8192.0*180.0; /* (deg/s) */ + + for (k = 0;k < MAXCODE;k++) + { + rtcm->ssr[sat-1].pbias[k] = pbias[k]; + rtcm->ssr[sat-1].stdpb[k] = (float)stdpb[k]; + } + } + return 20; +} + + +/* get signal index ----------------------------------------------------------*/ +void sigindex(int sys, const unsigned char *code, const int *freq, int n, + const char *opt, int *ind) +{ + int i, nex, pri, pri_h[8] = {0}, index[8] = {0}, ex[32] = {0}; + + /* test code priority */ + for (i = 0;i < n;i++) + { + if (!code[i]) continue; + + if (freq[i]>NFREQ) + { /* save as extended signal if freq > NFREQ */ + ex[i] = 1; + continue; + } + /* code priority */ + pri = getcodepri(sys, code[i], opt); + + /* select highest priority signal */ + if (pri>pri_h[freq[i]-1]) + { + if (index[freq[i]-1]) ex[index[freq[i]-1]-1] = 1; + pri_h[freq[i]-1] = pri; + index[freq[i]-1] = i+1; + } + else ex[i] = 1; + } + /* signal index in obs data */ + for (i = nex = 0;i < n;i++) + { + if (ex[i] == 0) ind[i] = freq[i]-1; + else if (nex < NEXOBS) ind[i] = NFREQ+nex++; + else + { /* no space in obs data */ + trace(2, "rtcm msm: no space in obs data sys=%d code=%d\n", sys, code[i]); + ind[i] = -1; + } +#if 0 + trace(2, "sig pos: sys=%d code=%d ex=%d ind=%d\n", sys, code[i], ex[i], ind[i]); +#endif + } +} + + +/* save obs data in msm message ----------------------------------------------*/ +void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, + const double *pr, const double *cp, const double *rr, + const double *rrf, const double *cnr, const int *lock, + const int *ex, const int *half) +{ + const char *sig[32]; + double tt, wl; + unsigned char code[32]; + char *msm_type = (char*)"", *q = NULL; + int i, j, k, type, prn, sat, fn, index = 0, freq[32], ind[32]; + + type = getbitu(rtcm->buff, 24, 12); + + switch (sys) + { + case SYS_GPS: msm_type = q = rtcm->msmtype[0]; break; + case SYS_GLO: msm_type = q = rtcm->msmtype[1]; break; + case SYS_GAL: msm_type = q = rtcm->msmtype[2]; break; + case SYS_QZS: msm_type = q = rtcm->msmtype[3]; break; + case SYS_SBS: msm_type = q = rtcm->msmtype[4]; break; + case SYS_BDS: msm_type = q = rtcm->msmtype[5]; break; + } + /* id to signal */ + for (i = 0;i < h->nsig;i++) + { + switch (sys) + { + case SYS_GPS: sig[i] = msm_sig_gps[h->sigs[i]-1]; break; + case SYS_GLO: sig[i] = msm_sig_glo[h->sigs[i]-1]; break; + case SYS_GAL: sig[i] = msm_sig_gal[h->sigs[i]-1]; break; + case SYS_QZS: sig[i] = msm_sig_qzs[h->sigs[i]-1]; break; + case SYS_SBS: sig[i] = msm_sig_sbs[h->sigs[i]-1]; break; + case SYS_BDS: sig[i] = msm_sig_cmp[h->sigs[i]-1]; break; + default: sig[i] = ""; break; + } + /* signal to rinex obs type */ + code[i] = obs2code(sig[i], freq+i); + + /* freqency index for beidou */ + if (sys == SYS_BDS) + { + if (freq[i] == 5) freq[i] = 2; /* B2 */ + else if (freq[i] == 4) freq[i] = 3; /* B3 */ + } + if (code[i] != CODE_NONE) + { + if (q) q += sprintf(q, "L%s%s", sig[i], i < h->nsig-1?", ":""); + } + else + { + if (q) q += sprintf(q, "(%d)%s", h->sigs[i], i < h->nsig-1?", ":""); + + trace(2, "rtcm3 %d: unknown signal id=%2d\n", type, h->sigs[i]); + } + } + trace(3, "rtcm3 %d: signals=%s\n", type, msm_type); + + /* get signal index */ + sigindex(sys, code, freq, h->nsig, rtcm->opt, ind); + + for (i = j = 0;i < h->nsat;i++) + { + prn = h->sats[i]; + if (sys == SYS_QZS) prn += MINPRNQZS-1; + else if (sys == SYS_SBS) prn += MINPRNSBS-1; + + if ((sat = satno(sys, prn))) + { + tt = timediff(rtcm->obs.data[0].time, rtcm->time); + if (rtcm->obsflag || fabs(tt)>1E-9) + { + rtcm->obs.n = rtcm->obsflag = 0; + } + index = obsindex(&rtcm->obs, rtcm->time, sat); + } + else + { + trace(2, "rtcm3 %d satellite error: prn=%d\n", type, prn); + } + for (k = 0;k < h->nsig;k++) + { + if (!h->cellmask[k+i*h->nsig]) continue; + + if (sat && index >= 0 && ind[k] >= 0) + { + /* satellite carrier wave length */ + wl = satwavelen(sat, freq[k]-1, &rtcm->nav); + + /* glonass wave length by extended info */ + if (sys == SYS_GLO && ex && ex[i] <= 13) + { + fn = ex[i]-7; + wl = SPEED_OF_LIGHT/((freq[k] == 2?FREQ2_GLO:FREQ1_GLO)+ + (freq[k] == 2?DFRQ2_GLO:DFRQ1_GLO)*fn); + } + /* pseudorange (m) */ + if (r[i] != 0.0 && pr[j]>-1E12) + { + rtcm->obs.data[index].P[ind[k]] = r[i]+pr[j]; + } + /* carrier-phase (cycle) */ + if (r[i] != 0.0 && cp[j]>-1E12 && wl>0.0) + { + rtcm->obs.data[index].L[ind[k]] = (r[i]+cp[j])/wl; + } + /* doppler (hz) */ + if (rr && rrf && rrf[j]>-1E12 && wl>0.0) + { + rtcm->obs.data[index].D[ind[k]] = (float)(-(rr[i]+rrf[j])/wl); + } + rtcm->obs.data[index].LLI[ind[k]] = + lossoflock(rtcm, sat, ind[k], lock[j])+(half[j]?3:0); + rtcm->obs.data[index].SNR [ind[k]] = (unsigned char)(cnr[j]*4.0); + rtcm->obs.data[index].code[ind[k]] = code[k]; + } + j++; + } + } +} + + + +/* decode type msm message header --------------------------------------------*/ +int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + msm_h_t *h, int *hsize) +{ + msm_h_t h0 = {0}; + double tow, tod; + char *msg; + int i = 24, j, dow, mask, staid, type, ncell = 0; + + type = getbitu(rtcm->buff, i, 12); i += 12; + + *h = h0; + if (i+157 <= rtcm->len*8) + { + staid = getbitu(rtcm->buff, i, 12); i += 12; + + if (sys == SYS_GLO) + { + dow = getbitu(rtcm->buff, i, 3); i += 3; + tod = getbitu(rtcm->buff, i, 27)*0.001; i += 27; + adjday_glot(rtcm, tod); + } + else if (sys == SYS_BDS) + { + tow = getbitu(rtcm->buff, i, 30)*0.001; i += 30; + tow += 14.0; /* BDT -> GPST */ + adjweek(rtcm, tow); + } + else + { + tow = getbitu(rtcm->buff, i, 30)*0.001; i += 30; + adjweek(rtcm, tow); + } + *sync = getbitu(rtcm->buff, i, 1); i += 1; + *iod = getbitu(rtcm->buff, i, 3); i += 3; + h->time_s = getbitu(rtcm->buff, i, 7); i += 7; + h->clk_str = getbitu(rtcm->buff, i, 2); i += 2; + h->clk_ext = getbitu(rtcm->buff, i, 2); i += 2; + h->smooth = getbitu(rtcm->buff, i, 1); i += 1; + h->tint_s = getbitu(rtcm->buff, i, 3); i += 3; + for (j = 1;j <= 64;j++) + { + mask = getbitu(rtcm->buff, i, 1); i += 1; + if (mask) h->sats[h->nsat++] = j; + } + for (j = 1;j <= 32;j++) + { + mask = getbitu(rtcm->buff, i, 1); i += 1; + if (mask) h->sigs[h->nsig++] = j; + } + } + else + { + trace(2, "rtcm3 %d length error: len=%d\n", type, rtcm->len); + return -1; + } + /* test station id */ + if (!test_staid(rtcm, staid)) return -1; + + if (h->nsat*h->nsig>64) + { + trace(2, "rtcm3 %d number of sats and sigs error: nsat=%d nsig=%d\n", + type, h->nsat, h->nsig); + return -1; + } + if (i+h->nsat*h->nsig>rtcm->len*8) + { + trace(2, "rtcm3 %d length error: len=%d nsat=%d nsig=%d\n", type, + rtcm->len, h->nsat, h->nsig); + return -1; + } + for (j = 0;j < h->nsat*h->nsig;j++) + { + h->cellmask[j] = getbitu(rtcm->buff, i, 1); i += 1; + if (h->cellmask[j]) ncell++; + } + *hsize = i; + + trace(4, "decode_head_msm: time=%s sys=%d staid=%d nsat=%d nsig=%d sync=%d iod=%d ncell=%d\n", + time_str(rtcm->time, 2), sys, staid, h->nsat, h->nsig, *sync, *iod, ncell); + + if (rtcm->outtype) + { + msg = rtcm->msgtype+strlen(rtcm->msgtype); + sprintf(msg, " staid=%4d %s nsat=%2d nsig=%2d iod=%2d ncell=%2d sync=%d", + staid, time_str(rtcm->time, 2), h->nsat, h->nsig, *iod, ncell, *sync); + } + return ncell; +} + + +/* decode unsupported msm message --------------------------------------------*/ +int decode_msm0(rtcm_t *rtcm, int sys) +{ + msm_h_t h = {0}; + int i, sync, iod; + if (decode_msm_head(rtcm, sys, &sync, &iod, &h, &i) < 0) return -1; + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode msm 4: full pseudorange and phaserange plus cnr --------------------*/ +int decode_msm4(rtcm_t *rtcm, int sys) +{ + msm_h_t h = {0}; + double r[64], pr[64], cp[64], cnr[64]; + int i, j, type, sync, iod, ncell, rng, rng_m, prv, cpv, lock[64], half[64]; + + type = getbitu(rtcm->buff, 24, 12); + + /* decode msm header */ + if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; + + if (i+h.nsat*18+ncell*48>rtcm->len*8) + { + trace(2, "rtcm3 %d length error: nsat=%d ncell=%d len=%d\n", type, h.nsat, + ncell, rtcm->len); + return -1; + } + for (j = 0;j < h.nsat;j++) r[j] = 0.0; + for (j = 0;j < ncell;j++) pr[j] = cp[j] = -1E16; + + /* decode satellite data */ + for (j = 0;j < h.nsat;j++) + { /* range */ + rng = getbitu(rtcm->buff, i, 8); i += 8; + if (rng != 255) r[j] = rng*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { + rng_m = getbitu(rtcm->buff, i, 10); i += 10; + if (r[j] != 0.0) r[j] += rng_m*P2_10*RANGE_MS; + } + /* decode signal data */ + for (j = 0;j < ncell;j++) + { /* pseudorange */ + prv = getbits(rtcm->buff, i, 15); i += 15; + if (prv != -16384) pr[j] = prv*TWO_N24*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* phaserange */ + cpv = getbits(rtcm->buff, i, 22); i += 22; + if (cpv != -2097152) cp[j] = cpv*TWO_N29*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* lock time */ + lock[j] = getbitu(rtcm->buff, i, 4); i += 4; + } + for (j = 0;j < ncell;j++) + { /* half-cycle ambiguity */ + half[j] = getbitu(rtcm->buff, i, 1); i += 1; + } + for (j = 0;j < ncell;j++) + { /* cnr */ + cnr[j] = getbitu(rtcm->buff, i, 6)*1.0; i += 6; + } + /* save obs data in msm message */ + save_msm_obs(rtcm, sys, &h, r, pr, cp, NULL, NULL, cnr, lock, NULL, half); + + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode msm 5: full pseudorange, phaserange, phaserangerate and cnr --------*/ +int decode_msm5(rtcm_t *rtcm, int sys) +{ + msm_h_t h = {0}; + double r[64], rr[64], pr[64], cp[64], rrf[64], cnr[64]; + int i, j, type, sync, iod, ncell, rng, rng_m, rate, prv, cpv, rrv, lock[64]; + int ex[64], half[64]; + + type = getbitu(rtcm->buff, 24, 12); + + /* decode msm header */ + if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; + + if (i+h.nsat*36+ncell*63>rtcm->len*8) + { + trace(2, "rtcm3 %d length error: nsat=%d ncell=%d len=%d\n", type, h.nsat, + ncell, rtcm->len); + return -1; + } + for (j = 0;j < h.nsat;j++) + { + r[j] = rr[j] = 0.0; ex[j] = 15; + } + for (j = 0;j < ncell;j++) pr[j] = cp[j] = rrf[j] = -1E16; + + /* decode satellite data */ + for (j = 0;j < h.nsat;j++) + { /* range */ + rng = getbitu(rtcm->buff, i, 8); i += 8; + if (rng != 255) r[j] = rng*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { /* extended info */ + ex[j] = getbitu(rtcm->buff, i, 4); i += 4; + } + for (j = 0;j < h.nsat;j++) + { + rng_m = getbitu(rtcm->buff, i, 10); i += 10; + if (r[j] != 0.0) r[j] += rng_m*P2_10*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { /* phaserangerate */ + rate = getbits(rtcm->buff, i, 14); i += 14; + if (rate != -8192) rr[j] = rate*1.0; + } + /* decode signal data */ + for (j = 0;j < ncell;j++) + { /* pseudorange */ + prv = getbits(rtcm->buff, i, 15); i += 15; + if (prv != -16384) pr[j] = prv*TWO_N24*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* phaserange */ + cpv = getbits(rtcm->buff, i, 22); i += 22; + if (cpv != -2097152) cp[j] = cpv*TWO_N29*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* lock time */ + lock[j] = getbitu(rtcm->buff, i, 4); i += 4; + } + for (j = 0;j < ncell;j++) + { /* half-cycle ambiguity */ + half[j] = getbitu(rtcm->buff, i, 1); i += 1; + } + for (j = 0;j < ncell;j++) + { /* cnr */ + cnr[j] = getbitu(rtcm->buff, i, 6)*1.0; i += 6; + } + for (j = 0;j < ncell;j++) + { /* phaserangerate */ + rrv = getbits(rtcm->buff, i, 15); i += 15; + if (rrv != -16384) rrf[j] = rrv*0.0001; + } + /* save obs data in msm message */ + save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half); + + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode msm 6: full pseudorange and phaserange plus cnr (high-res) ---------*/ +int decode_msm6(rtcm_t *rtcm, int sys) +{ + msm_h_t h = {0}; + double r[64], pr[64], cp[64], cnr[64]; + int i, j, type, sync, iod, ncell, rng, rng_m, prv, cpv, lock[64], half[64]; + + type = getbitu(rtcm->buff, 24, 12); + + /* decode msm header */ + if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; + + if (i+h.nsat*18+ncell*65>rtcm->len*8) + { + trace(2, "rtcm3 %d length error: nsat=%d ncell=%d len=%d\n", type, h.nsat, + ncell, rtcm->len); + return -1; + } + for (j = 0;j < h.nsat;j++) r[j] = 0.0; + for (j = 0;j < ncell;j++) pr[j] = cp[j] = -1E16; + + /* decode satellite data */ + for (j = 0;j < h.nsat;j++) + { /* range */ + rng = getbitu(rtcm->buff, i, 8); i += 8; + if (rng != 255) r[j] = rng*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { + rng_m = getbitu(rtcm->buff, i, 10); i += 10; + if (r[j] != 0.0) r[j] += rng_m*P2_10*RANGE_MS; + } + /* decode signal data */ + for (j = 0;j < ncell;j++) + { /* pseudorange */ + prv = getbits(rtcm->buff, i, 20); i += 20; + if (prv != -524288) pr[j] = prv*TWO_N29*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* phaserange */ + cpv = getbits(rtcm->buff, i, 24); i += 24; + if (cpv != -8388608) cp[j] = cpv*TWO_N31*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* lock time */ + lock[j] = getbitu(rtcm->buff, i, 10); i += 10; + } + for (j = 0;j < ncell;j++) + { /* half-cycle ambiguity */ + half[j] = getbitu(rtcm->buff, i, 1); i += 1; + } + for (j = 0;j < ncell;j++) + { /* cnr */ + cnr[j] = getbitu(rtcm->buff, i, 10)*0.0625; i += 10; + } + /* save obs data in msm message */ + save_msm_obs(rtcm, sys, &h, r, pr, cp, NULL, NULL, cnr, lock, NULL, half); + + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode msm 7: full pseudorange, phaserange, phaserangerate and cnr (h-res) */ +int decode_msm7(rtcm_t *rtcm, int sys) +{ + msm_h_t h = {0}; + double r[64], rr[64], pr[64], cp[64], rrf[64], cnr[64]; + int i, j, type, sync, iod, ncell, rng, rng_m, rate, prv, cpv, rrv, lock[64]; + int ex[64], half[64]; + + type = getbitu(rtcm->buff, 24, 12); + + /* decode msm header */ + if ((ncell = decode_msm_head(rtcm, sys, &sync, &iod, &h, &i)) < 0) return -1; + + if (i+h.nsat*36+ncell*80 > rtcm->len*8) + { + trace(2, "rtcm3 %d length error: nsat=%d ncell=%d len=%d\n", type, h.nsat, + ncell, rtcm->len); + return -1; + } + for (j = 0;j < h.nsat;j++) + { + r[j] = rr[j] = 0.0; ex[j] = 15; + } + for (j = 0;j < ncell;j++) pr[j] = cp[j] = rrf[j] = -1E16; + + /* decode satellite data */ + for (j = 0;j < h.nsat;j++) + { /* range */ + rng = getbitu(rtcm->buff, i, 8); i += 8; + if (rng != 255) r[j] = rng*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { /* extended info */ + ex[j] = getbitu(rtcm->buff, i, 4); i += 4; + } + for (j = 0;j < h.nsat;j++) + { + rng_m = getbitu(rtcm->buff, i, 10); i += 10; + if (r[j] != 0.0) r[j] += rng_m*P2_10*RANGE_MS; + } + for (j = 0;j < h.nsat;j++) + { /* phaserangerate */ + rate = getbits(rtcm->buff, i, 14); i += 14; + if (rate != -8192) rr[j] = rate*1.0; + } + /* decode signal data */ + for (j = 0;j < ncell;j++) + { /* pseudorange */ + prv = getbits(rtcm->buff, i, 20); i += 20; + if (prv != -524288) pr[j] = prv*TWO_N29*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* phaserange */ + cpv = getbits(rtcm->buff, i, 24); i += 24; + if (cpv != -8388608) cp[j] = cpv*TWO_N31*RANGE_MS; + } + for (j = 0;j < ncell;j++) + { /* lock time */ + lock[j] = getbitu(rtcm->buff, i, 10); i += 10; + } + for (j = 0;j < ncell;j++) + { /* half-cycle amiguity */ + half[j] = getbitu(rtcm->buff, i, 1); i += 1; + } + for (j = 0;j < ncell;j++) + { /* cnr */ + cnr[j] = getbitu(rtcm->buff, i, 10)*0.0625; i += 10; + } + for (j = 0;j < ncell;j++) + { /* phaserangerate */ + rrv = getbits(rtcm->buff, i, 15); i += 15; + if (rrv != -16384) rrf[j] = rrv*0.0001; + } + /* save obs data in msm message */ + save_msm_obs(rtcm, sys, &h, r, pr, cp, rr, rrf, cnr, lock, ex, half); + + rtcm->obsflag = !sync; + return sync?0:1; +} + + +/* decode type 1230: glonass L1 and L2 code-phase biases ---------------------*/ +int decode_type1230(rtcm_t *rtcm) +{ + trace(2, "rtcm3 1230: not supported message\n"); + return 0; +} + + +/* decode rtcm ver.3 message -------------------------------------------------*/ +int decode_rtcm3(rtcm_t *rtcm) +{ + double tow; + int ret = 0, type = getbitu(rtcm->buff, 24, 12), week; + + trace(3, "decode_rtcm3: len=%3d type=%d\n", rtcm->len, type); + + if (rtcm->outtype) + { + sprintf(rtcm->msgtype, "RTCM %4d (%4d):", type, rtcm->len); + } + /* real-time input option */ + if (strstr(rtcm->opt, "-RT_INP")) + { + tow = time2gpst(utc2gpst(timeget()), &week); + rtcm->time = gpst2time(week, floor(tow)); + } + switch (type) + { + case 1001: ret = decode_type1001(rtcm); break; /* not supported */ + case 1002: ret = decode_type1002(rtcm); break; + case 1003: ret = decode_type1003(rtcm); break; /* not supported */ + case 1004: ret = decode_type1004(rtcm); break; + case 1005: ret = decode_type1005(rtcm); break; + case 1006: ret = decode_type1006(rtcm); break; + case 1007: ret = decode_type1007(rtcm); break; + case 1008: ret = decode_type1008(rtcm); break; + case 1009: ret = decode_type1009(rtcm); break; /* not supported */ + case 1010: ret = decode_type1010(rtcm); break; + case 1011: ret = decode_type1011(rtcm); break; /* not supported */ + case 1012: ret = decode_type1012(rtcm); break; + case 1013: ret = decode_type1013(rtcm); break; /* not supported */ + case 1019: ret = decode_type1019(rtcm); break; + case 1020: ret = decode_type1020(rtcm); break; + case 1021: ret = decode_type1021(rtcm); break; /* not supported */ + case 1022: ret = decode_type1022(rtcm); break; /* not supported */ + case 1023: ret = decode_type1023(rtcm); break; /* not supported */ + case 1024: ret = decode_type1024(rtcm); break; /* not supported */ + case 1025: ret = decode_type1025(rtcm); break; /* not supported */ + case 1026: ret = decode_type1026(rtcm); break; /* not supported */ + case 1027: ret = decode_type1027(rtcm); break; /* not supported */ + case 1029: ret = decode_type1029(rtcm); break; + case 1030: ret = decode_type1030(rtcm); break; /* not supported */ + case 1031: ret = decode_type1031(rtcm); break; /* not supported */ + case 1032: ret = decode_type1032(rtcm); break; /* not supported */ + case 1033: ret = decode_type1033(rtcm); break; + case 1034: ret = decode_type1034(rtcm); break; /* not supported */ + case 1035: ret = decode_type1035(rtcm); break; /* not supported */ + case 1037: ret = decode_type1037(rtcm); break; /* not supported */ + case 1038: ret = decode_type1038(rtcm); break; /* not supported */ + case 1039: ret = decode_type1039(rtcm); break; /* not supported */ + case 1044: ret = decode_type1044(rtcm); break; + case 1045: ret = decode_type1045(rtcm); break; + case 1046: ret = decode_type1046(rtcm); break; /* extension for IGS MGEX */ + case 1047: ret = decode_type1047(rtcm); break; /* beidou ephemeris (tentative mt) */ + case 63: ret = decode_type63 (rtcm); break; /* beidou ephemeris (rtcm draft) */ + case 1057: ret = decode_ssr1(rtcm, SYS_GPS); break; + case 1058: ret = decode_ssr2(rtcm, SYS_GPS); break; + case 1059: ret = decode_ssr3(rtcm, SYS_GPS); break; + case 1060: ret = decode_ssr4(rtcm, SYS_GPS); break; + case 1061: ret = decode_ssr5(rtcm, SYS_GPS); break; + case 1062: ret = decode_ssr6(rtcm, SYS_GPS); break; + case 1063: ret = decode_ssr1(rtcm, SYS_GLO); break; + case 1064: ret = decode_ssr2(rtcm, SYS_GLO); break; + case 1065: ret = decode_ssr3(rtcm, SYS_GLO); break; + case 1066: ret = decode_ssr4(rtcm, SYS_GLO); break; + case 1067: ret = decode_ssr5(rtcm, SYS_GLO); break; + case 1068: ret = decode_ssr6(rtcm, SYS_GLO); break; + case 1071: ret = decode_msm0(rtcm, SYS_GPS); break; /* not supported */ + case 1072: ret = decode_msm0(rtcm, SYS_GPS); break; /* not supported */ + case 1073: ret = decode_msm0(rtcm, SYS_GPS); break; /* not supported */ + case 1074: ret = decode_msm4(rtcm, SYS_GPS); break; + case 1075: ret = decode_msm5(rtcm, SYS_GPS); break; + case 1076: ret = decode_msm6(rtcm, SYS_GPS); break; + case 1077: ret = decode_msm7(rtcm, SYS_GPS); break; + case 1081: ret = decode_msm0(rtcm, SYS_GLO); break; /* not supported */ + case 1082: ret = decode_msm0(rtcm, SYS_GLO); break; /* not supported */ + case 1083: ret = decode_msm0(rtcm, SYS_GLO); break; /* not supported */ + case 1084: ret = decode_msm4(rtcm, SYS_GLO); break; + case 1085: ret = decode_msm5(rtcm, SYS_GLO); break; + case 1086: ret = decode_msm6(rtcm, SYS_GLO); break; + case 1087: ret = decode_msm7(rtcm, SYS_GLO); break; + case 1091: ret = decode_msm0(rtcm, SYS_GAL); break; /* not supported */ + case 1092: ret = decode_msm0(rtcm, SYS_GAL); break; /* not supported */ + case 1093: ret = decode_msm0(rtcm, SYS_GAL); break; /* not supported */ + case 1094: ret = decode_msm4(rtcm, SYS_GAL); break; + case 1095: ret = decode_msm5(rtcm, SYS_GAL); break; + case 1096: ret = decode_msm6(rtcm, SYS_GAL); break; + case 1097: ret = decode_msm7(rtcm, SYS_GAL); break; + case 1101: ret = decode_msm0(rtcm, SYS_SBS); break; /* not supported */ + case 1102: ret = decode_msm0(rtcm, SYS_SBS); break; /* not supported */ + case 1103: ret = decode_msm0(rtcm, SYS_SBS); break; /* not supported */ + case 1104: ret = decode_msm4(rtcm, SYS_SBS); break; + case 1105: ret = decode_msm5(rtcm, SYS_SBS); break; + case 1106: ret = decode_msm6(rtcm, SYS_SBS); break; + case 1107: ret = decode_msm7(rtcm, SYS_SBS); break; + case 1111: ret = decode_msm0(rtcm, SYS_QZS); break; /* not supported */ + case 1112: ret = decode_msm0(rtcm, SYS_QZS); break; /* not supported */ + case 1113: ret = decode_msm0(rtcm, SYS_QZS); break; /* not supported */ + case 1114: ret = decode_msm4(rtcm, SYS_QZS); break; + case 1115: ret = decode_msm5(rtcm, SYS_QZS); break; + case 1116: ret = decode_msm6(rtcm, SYS_QZS); break; + case 1117: ret = decode_msm7(rtcm, SYS_QZS); break; + case 1121: ret = decode_msm0(rtcm, SYS_BDS); break; /* not supported */ + case 1122: ret = decode_msm0(rtcm, SYS_BDS); break; /* not supported */ + case 1123: ret = decode_msm0(rtcm, SYS_BDS); break; /* not supported */ + case 1124: ret = decode_msm4(rtcm, SYS_BDS); break; + case 1125: ret = decode_msm5(rtcm, SYS_BDS); break; + case 1126: ret = decode_msm6(rtcm, SYS_BDS); break; + case 1127: ret = decode_msm7(rtcm, SYS_BDS); break; + case 1230: ret = decode_type1230(rtcm); break; /* not supported */ + case 1240: ret = decode_ssr1(rtcm, SYS_GAL); break; + case 1241: ret = decode_ssr2(rtcm, SYS_GAL); break; + case 1242: ret = decode_ssr3(rtcm, SYS_GAL); break; + case 1243: ret = decode_ssr4(rtcm, SYS_GAL); break; + case 1244: ret = decode_ssr5(rtcm, SYS_GAL); break; + case 1245: ret = decode_ssr6(rtcm, SYS_GAL); break; + case 1246: ret = decode_ssr1(rtcm, SYS_QZS); break; + case 1247: ret = decode_ssr2(rtcm, SYS_QZS); break; + case 1248: ret = decode_ssr3(rtcm, SYS_QZS); break; + case 1249: ret = decode_ssr4(rtcm, SYS_QZS); break; + case 1250: ret = decode_ssr5(rtcm, SYS_QZS); break; + case 1251: ret = decode_ssr6(rtcm, SYS_QZS); break; + case 1252: ret = decode_ssr1(rtcm, SYS_SBS); break; + case 1253: ret = decode_ssr2(rtcm, SYS_SBS); break; + case 1254: ret = decode_ssr3(rtcm, SYS_SBS); break; + case 1255: ret = decode_ssr4(rtcm, SYS_SBS); break; + case 1256: ret = decode_ssr5(rtcm, SYS_SBS); break; + case 1257: ret = decode_ssr6(rtcm, SYS_SBS); break; + case 1258: ret = decode_ssr1(rtcm, SYS_BDS); break; + case 1259: ret = decode_ssr2(rtcm, SYS_BDS); break; + case 1260: ret = decode_ssr3(rtcm, SYS_BDS); break; + case 1261: ret = decode_ssr4(rtcm, SYS_BDS); break; + case 1262: ret = decode_ssr5(rtcm, SYS_BDS); break; + case 1263: ret = decode_ssr6(rtcm, SYS_BDS); break; + case 2065: ret = decode_ssr7(rtcm, SYS_GPS); break; /* tentative */ + case 2066: ret = decode_ssr7(rtcm, SYS_GLO); break; /* tentative */ + case 2067: ret = decode_ssr7(rtcm, SYS_GAL); break; /* tentative */ + case 2068: ret = decode_ssr7(rtcm, SYS_QZS); break; /* tentative */ + case 2070: ret = decode_ssr7(rtcm, SYS_BDS); break; /* tentative */ + } + if (ret >= 0) + { + type -= 1000; + if ( 1 <= type && type <= 299) rtcm->nmsg3[type ]++; /* 1001-1299 */ + else if (1000 <= type && type <= 1099) rtcm->nmsg3[type-700]++; /* 2000-2099 */ + else rtcm->nmsg3[0]++; + } + return ret; +} diff --git a/src/algorithms/libs/rtklib/rtklib_rtcm3.h b/src/algorithms/libs/rtklib/rtklib_rtcm3.h new file mode 100644 index 000000000..ff179a9de --- /dev/null +++ b/src/algorithms/libs/rtklib/rtklib_rtcm3.h @@ -0,0 +1,262 @@ +/*! + * \file rtklib_rtcm3.h + * \brief RTCM v3 functions headers + * \authors
    + *
  • 2007-2013, T. Takasu + *
  • 2017, Javier Arribas + *
  • 2017, Carles Fernandez + *
+ * + * This is a derived work from RTKLIB http://www.rtklib.com/ + * The original source code at https://github.com/tomojitakasu/RTKLIB is + * released under the BSD 2-clause license with an additional exclusive clause + * that does not apply here. This additional clause is reproduced below: + * + * " The software package includes some companion executive binaries or shared + * libraries necessary to execute APs on Windows. These licenses succeed to the + * original ones of these software. " + * + * Neither the executive binaries nor the shared libraries are required by, used + * or included in GNSS-SDR. + * + * ------------------------------------------------------------------------- + * Copyright (C) 2007-2013, T. Takasu + * Copyright (C) 2017, Javier Arribas + * Copyright (C) 2017, Carles Fernandez + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + * + *----------------------------------------------------------------------------*/ + +#ifndef GNSS_SDR_RTKLIB_RTCM3_H_ +#define GNSS_SDR_RTKLIB_RTCM3_H_ + +#include "rtklib.h" + +/* constants -----------------------------------------------------------------*/ + +const double PRUNIT_GPS = 299792.458; /* rtcm ver.3 unit of gps pseudorange (m) */ +const double PRUNIT_GLO = 599584.916; /* rtcm ver.3 unit of glonass pseudorange (m) */ +const double RANGE_MS = SPEED_OF_LIGHT * 0.001; /* range in 1 ms */ + +#define P2_10 0.0009765625 /* 2^-10 */ +#define P2_34 5.820766091346740E-11 /* 2^-34 */ +#define P2_46 1.421085471520200E-14 /* 2^-46 */ +#define P2_59 1.734723475976810E-18 /* 2^-59 */ +#define P2_66 1.355252715606880E-20 /* 2^-66 */ + +/* type definition -----------------------------------------------------------*/ + +typedef struct { /* multi-signal-message header type */ + unsigned char iod; /* issue of data station */ + unsigned char time_s; /* cumulative session transmitting time */ + unsigned char clk_str; /* clock steering indicator */ + unsigned char clk_ext; /* external clock indicator */ + unsigned char smooth; /* divergence free smoothing indicator */ + unsigned char tint_s; /* soothing interval */ + unsigned char nsat,nsig; /* number of satellites/signals */ + unsigned char sats[64]; /* satellites */ + unsigned char sigs[32]; /* signals */ + unsigned char cellmask[64]; /* cell mask */ +} msm_h_t; + +/* msm signal id table -------------------------------------------------------*/ +const char *msm_sig_gps[32]={ + /* GPS: ref [13] table 3.5-87, ref [14][15] table 3.5-91 */ + "" ,"1C","1P","1W","1Y","1M","" ,"2C","2P","2W","2Y","2M", /* 1-12 */ + "" ,"" ,"2S","2L","2X","" ,"" ,"" ,"" ,"5I","5Q","5X", /* 13-24 */ + "" ,"" ,"" ,"" ,"" ,"1S","1L","1X" /* 25-32 */ +}; + + +const char *msm_sig_glo[32]={ + /* GLONASS: ref [13] table 3.5-93, ref [14][15] table 3.5-97 */ + "" ,"1C","1P","" ,"" ,"" ,"" ,"2C","2P","" ,"3I","3Q", + "3X","" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" , + "" ,"" ,"" ,"" ,"" ,"" ,"" ,"" +}; + + +const char *msm_sig_gal[32]={ + /* Galileo: ref [15] table 3.5-100 */ + "" ,"1C","1A","1B","1X","1Z","" ,"6C","6A","6B","6X","6Z", + "" ,"7I","7Q","7X","" ,"8I","8Q","8X","" ,"5I","5Q","5X", + "" ,"" ,"" ,"" ,"" ,"" ,"" ,"" +}; + + +const char *msm_sig_qzs[32]={ + /* QZSS: ref [15] table 3.5-103 */ + "" ,"1C","" ,"" ,"" ,"" ,"" ,"" ,"6S","6L","6X","" , + "" ,"" ,"2S","2L","2X","" ,"" ,"" ,"" ,"5I","5Q","5X", + "" ,"" ,"" ,"" ,"" ,"1S","1L","1X" +}; + + +const char *msm_sig_sbs[32]={ + /* SBAS: ref [13] table 3.5-T+005 */ + "" ,"1C","" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" , + "" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"" ,"5I","5Q","5X", + "" ,"" ,"" ,"" ,"" ,"" ,"" ,"" +}; + + +const char *msm_sig_cmp[32]={ + /* BeiDou: ref [15] table 3.5-106 */ + "" ,"1I","1Q","1X","" ,"" ,"" ,"6I","6Q","6X","" ,"" , + "" ,"7I","7Q","7X","" ,"" ,"" ,"" ,"" ,"" ,"" ,"" , + "" ,"" ,"" ,"" ,"" ,"" ,"" ,"" +}; + + +/* ssr update intervals ------------------------------------------------------*/ +const double ssrudint[16]={ + 1,2,5,10,15,30,60,120,240,300,600,900,1800,3600,7200,10800 +}; + + +/* ssr 3 and 7 signal and tracking mode ids ----------------------------------*/ +const int codes_gps[]={ + CODE_L1C,CODE_L1P,CODE_L1W,CODE_L1Y,CODE_L1M,CODE_L2C,CODE_L2D,CODE_L2S, + CODE_L2L,CODE_L2X,CODE_L2P,CODE_L2W,CODE_L2Y,CODE_L2M,CODE_L5I,CODE_L5Q, + CODE_L5X +}; + + +const int codes_glo[]={ + CODE_L1C,CODE_L1P,CODE_L2C,CODE_L2P +}; + + +const int codes_gal[]={ + CODE_L1A,CODE_L1B,CODE_L1C,CODE_L1X,CODE_L1Z,CODE_L5I,CODE_L5Q,CODE_L5X, + CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L8I,CODE_L8Q,CODE_L8X,CODE_L6A,CODE_L6B, + CODE_L6C,CODE_L6X,CODE_L6Z +}; + + +const int codes_qzs[]={ + CODE_L1C,CODE_L1S,CODE_L1L,CODE_L2S,CODE_L2L,CODE_L2X,CODE_L5I,CODE_L5Q, + CODE_L5X,CODE_L6S,CODE_L6L,CODE_L6X,CODE_L1X +}; + + +const int codes_bds[]={ + CODE_L1I,CODE_L1Q,CODE_L1X,CODE_L7I,CODE_L7Q,CODE_L7X,CODE_L6I,CODE_L6Q, + CODE_L6X +}; + + +const int codes_sbs[]={ + CODE_L1C,CODE_L5I,CODE_L5Q,CODE_L5X +}; + + +double getbitg(const unsigned char *buff, int pos, int len); +void adjweek(rtcm_t *rtcm, double tow); +int adjbdtweek(int week); +void adjday_glot(rtcm_t *rtcm, double tod); +double adjcp(rtcm_t *rtcm, int sat, int freq, double cp); +int lossoflock(rtcm_t *rtcm, int sat, int freq, int lock); +unsigned char snratio(double snr); +int obsindex(obs_t *obs, gtime_t time, int sat); +int test_staid(rtcm_t *rtcm, int staid); + +int decode_head1001(rtcm_t *rtcm, int *sync); +int decode_type1001(rtcm_t *rtcm); +int decode_type1002(rtcm_t *rtcm); +int decode_type1003(rtcm_t *rtcm); +int decode_type1004(rtcm_t *rtcm); +double getbits_38(const unsigned char *buff, int pos); +int decode_type1005(rtcm_t *rtcm); + +int decode_type1006(rtcm_t *rtcm); +int decode_type1007(rtcm_t *rtcm); +int decode_type1008(rtcm_t *rtcm); +int decode_head1009(rtcm_t *rtcm, int *sync); + +int decode_type1009(rtcm_t *rtcm); +int decode_type1010(rtcm_t *rtcm); +int decode_type1011(rtcm_t *rtcm); +int decode_type1012(rtcm_t *rtcm); +int decode_type1013(rtcm_t *rtcm); +int decode_type1019(rtcm_t *rtcm); +int decode_type1020(rtcm_t *rtcm); +int decode_type1021(rtcm_t *rtcm); +int decode_type1022(rtcm_t *rtcm); +int decode_type1023(rtcm_t *rtcm); +int decode_type1024(rtcm_t *rtcm); +int decode_type1025(rtcm_t *rtcm); +int decode_type1026(rtcm_t *rtcm); +int decode_type1027(rtcm_t *rtcm); +int decode_type1029(rtcm_t *rtcm); +int decode_type1030(rtcm_t *rtcm); +int decode_type1031(rtcm_t *rtcm); +int decode_type1032(rtcm_t *rtcm); +int decode_type1033(rtcm_t *rtcm); +int decode_type1034(rtcm_t *rtcm); +int decode_type1035(rtcm_t *rtcm); +int decode_type1037(rtcm_t *rtcm); +int decode_type1038(rtcm_t *rtcm); +int decode_type1039(rtcm_t *rtcm); +int decode_type1044(rtcm_t *rtcm); +int decode_type1045(rtcm_t *rtcm); +int decode_type1046(rtcm_t *rtcm); +int decode_type1047(rtcm_t *rtcm); +int decode_type1063(rtcm_t *rtcm); +int decode_ssr1_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *refd, int *hsize); +int decode_ssr2_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *hsize); +int decode_ssr7_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + double *udint, int *dispe, int *mw, int *hsize); +int decode_ssr1(rtcm_t *rtcm, int sys); +int decode_ssr2(rtcm_t *rtcm, int sys); +int decode_ssr3(rtcm_t *rtcm, int sys); +int decode_ssr4(rtcm_t *rtcm, int sys); +int decode_ssr5(rtcm_t *rtcm, int sys); +int decode_ssr6(rtcm_t *rtcm, int sys); +int decode_ssr7(rtcm_t *rtcm, int sys); +void sigindex(int sys, const unsigned char *code, const int *freq, int n, + const char *opt, int *ind); +void save_msm_obs(rtcm_t *rtcm, int sys, msm_h_t *h, const double *r, + const double *pr, const double *cp, const double *rr, + const double *rrf, const double *cnr, const int *lock, + const int *ex, const int *half); +int decode_msm_head(rtcm_t *rtcm, int sys, int *sync, int *iod, + msm_h_t *h, int *hsize); + +int decode_msm0(rtcm_t *rtcm, int sys); +int decode_msm4(rtcm_t *rtcm, int sys); +int decode_msm5(rtcm_t *rtcm, int sys); +int decode_msm6(rtcm_t *rtcm, int sys); +int decode_msm7(rtcm_t *rtcm, int sys); +int decode_type1230(rtcm_t *rtcm); + +int decode_rtcm3(rtcm_t *rtcm); + + +#endif