#include "rtklib.h" /* function prototypes -------------------------------------------------------*/ extern int decode_rtcm3(rtcm_t *rtcm); /* 定义静态数组替换动态数组 */ #ifdef STATIC #define RTCM_STATIC #endif /* 定义静态数组替换动态数组 */ /* constants -----------------------------------------------------------------*/ #define RTCM3PREAMB 0xD3 /* rtcm ver.3 frame preamble */ /* 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) *-----------------------------------------------------------------------------*/ extern int init_rtcm(rtcm_t *rtcm) { gtime_t time0 = {0}; obsd_t data0 = {{0}}; eph_t eph0 = {0, -1, -1}; #ifdef ENAGLO geph_t geph0 = {0, -1}; #endif // ssr_t ssr0={{{0}}}; int i, j; trace(4, "init_rtcm:\n"); rtcm->staid = rtcm->stah = rtcm->seqno = 0; // 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;issr[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->nbit = rtcm->len = 0; rtcm->nbyte = 0; // rtcm->word=0; // for (i=0;i<100;i++) rtcm->nmsg2[i]=0; for (i = 0; i < 400; i++) rtcm->nmsg3[i] = 0; rtcm->obs.data = NULL; rtcm->nav.eph = NULL; rtcm->nav.geph = NULL; /* reallocate memory for observation and ephemeris buffer */ #ifndef ENAGLO if (!(rtcm->obs.data = (obsd_t *)malloc(sizeof(obsd_t) * MAXOBS)) || !(rtcm->nav.eph = (eph_t *)malloc(sizeof(eph_t) * MAXSAT * 1))) #else 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))) #endif { free_rtcm(rtcm); return 0; } rtcm->obs.n = 0; rtcm->nav.n = MAXSAT * 1; /* 接收机星历 (当前星历) */ rtcm->nav.ng = MAXPRNGLO; for (i = 0; i < MAXOBS; i++) rtcm->obs.data[i] = data0; for (i = 0; i < MAXSAT * 1; i++) rtcm->nav.eph[i] = eph0; #ifdef ENAGLO for (i = 0; i < MAXPRNGLO; i++) rtcm->nav.geph[i] = geph0; #endif return 1; } /* 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) *-----------------------------------------------------------------------------*/ extern int init_rtcm_static(rtcm_t *rtcm, char index) { gtime_t time0 = {0}; obsd_t data0 = {{0}}; eph_t eph0 = {0, -1, -1}; extern obsd_t RTCM_obsd_r[MAXOBS]; extern obsd_t RTCM_obsd_b[MAXOBS]; extern eph_t RTCM_eph_r[MAXSAT]; #ifdef ENAGLO extern eph_t RTCM_geph[MAXPRNGLO]; geph_t geph0 = {0, -1}; #endif // ssr_t ssr0={{{0}}}; int i, j; trace(4, "init_rtcm:\n"); rtcm->staid = rtcm->stah = rtcm->seqno = 0; // 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;issr[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->nbit = rtcm->len = 0; rtcm->nbyte = 0; // rtcm->word=0; // for (i=0;i<100;i++) rtcm->nmsg2[i]=0; for (i = 0; i < 400; i++) rtcm->nmsg3[i] = 0; rtcm->obs.data = NULL; rtcm->nav.eph = NULL; rtcm->nav.geph = NULL; /* reallocate memory for observation and ephemeris buffer */ if (index == 0) /* 接收机静态 */ { memset(RTCM_obsd_r,0,MAXOBS); memset(RTCM_eph_r,0,MAXSAT); rtcm->obs.data = RTCM_obsd_r; rtcm->nav.eph = RTCM_eph_r; rtcm->obs.n = 0; rtcm->nav.n = MAXSAT * 1; /* 接收机星历 (当前星历) */ rtcm->nav.ng = MAXPRNGLO; for (i = 0; i < MAXOBS; i++) rtcm->obs.data[i] = data0; for (i = 0; i < MAXSAT * 1; i++) rtcm->nav.eph[i] = eph0; } else /* 基准站静态 */ { memset(RTCM_obsd_b,0,MAXOBS); rtcm->obs.data = RTCM_obsd_b; rtcm->obs.n = 0; rtcm->nav.n = 0; /* 基准站最大星历数量 */ rtcm->nav.ng = 0; for (i = 0; i < MAXOBS; i++) rtcm->obs.data[i] = data0; } #ifdef ENAGLO if (index == 0) { memset(RTCM_geph_r,0,MAXPRNGLO); rtcm->nav.geph = RTCM_geph_r; for (i = 0; i < MAXPRNGLO; i++) rtcm->nav.geph[i] = geph0; } #endif return 1; } /* free rtcm control ---------------------------------------------------------- * free observation and ephemeris buffer in rtcm control struct * args : rtcm_t *raw IO rtcm control struct * return : none *-----------------------------------------------------------------------------*/ extern void free_rtcm(rtcm_t *rtcm) { trace(4, "free_rtcm:\n"); /* free memory for observation and ephemeris buffer */ #ifndef RTCM_STATIC free(rtcm->obs.data); rtcm->obs.data = NULL; free(rtcm->nav.eph); rtcm->nav.eph = NULL; free(rtcm->nav.geph); rtcm->nav.geph = NULL; #endif rtcm->nav.ng = 0; rtcm->obs.n = 0; rtcm->nav.n = 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 * uint8_t 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 (default: only new) * -STA=nnn : input only message with STAID=nnn (default: all) * -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,...) * -ILss : select signal ss for IRN MSM (ss=5A,9A,...) * -GALINAV : select I/NAV for Galileo ephemeris (default: all) * -GALFNAV : select F/NAV for Galileo ephemeris (default: all) * * supported RTCM 3 messages (ref [7][10][15][16][17][18]) * * TYPE : GPS GLONASS Galileo QZSS BDS SBAS NavIC * ---------------------------------------------------------------------- * OBS COMP L1 : 1001~ 1009~ - - - - - * FULL L1 : 1002 1010 - - - - - * COMP L1L2: 1003~ 1011~ - - - - - * FULL L1L2: 1004 1012 - - - - - * * NAV : 1019 1020 1045** 1044 1042 - 1041 * - - 1046** - 63* - - * * MSM 1 : 1071~ 1081~ 1091~ 1111~ 1121~ 1101~ 1131~ * 2 : 1072~ 1082~ 1092~ 1112~ 1122~ 1102~ 1132~ * 3 : 1073~ 1083~ 1093~ 1113~ 1123~ 1103~ 1133~ * 4 : 1074 1084 1094 1114 1124 1104 1134 * 5 : 1075 1085 1095 1115 1125 1105 1135 * 6 : 1076 1086 1096 1116 1126 1106 1136 * 7 : 1077 1087 1097 1117 1127 1107 1137 * * SSR ORBIT : 1057 1063 1240* 1246* 1258* - - * CLOCK : 1058 1064 1241* 1247* 1259* - - * CODE BIAS: 1059 1065 1242* 1248* 1260* - - * OBT/CLK : 1060 1066 1243* 1249* 1261* - - * URA : 1061 1067 1244* 1250* 1262* - - * HR-CLOCK : 1062 1068 1245* 1251* 1263* - - * PHAS BIAS: 11* - 12* 13* 14* - - * * ANT/RCV INFO : 1007 1008 1033 * STA POSITION : 1005 1006 * * PROPRIETARY : 4076 (IGS) * ---------------------------------------------------------------------- * (* draft, ** 1045:F/NAV,1046:I/NAV, ~ 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. * * RTCM 3 message format: * +----------+--------+-----------+--------------------+----------+ * | preamble | 000000 | length | data message | parity | * +----------+--------+-----------+--------------------+----------+ * |<-- 8 --->|<- 6 -->|<-- 10 --->|<--- length x 8 --->|<-- 24 -->| * *-----------------------------------------------------------------------------*/ extern int input_rtcm3(rtcm_t *rtcm, uint8_t 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 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 *-----------------------------------------------------------------------------*/ extern 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, (uint8_t)data))) return ret; } return 0; /* return at every 4k bytes */ }