From 511410b83040ff85dfea001fb6bce1972b31d126 Mon Sep 17 00:00:00 2001 From: oaq Date: Sat, 11 Oct 2025 18:08:42 +1100 Subject: [PATCH] rtcm3: support message type 1013 system parameters This also moves the set of message types and their intervals into the rtcm_t structure from the strconv_t structure so that they are available to for the system parameters message. --- src/rtcm3.c | 51 ++++++++++++++++++++++++++++--- src/rtcm3e.c | 26 ++++++++++++++++ src/rtklib.h | 6 ++-- src/streamsvr.c | 64 +++++++++++++++++++------------------- util/rnx2rtcm/rnx2rtcm.c | 66 +++++++++++++++++++++++++++++++++++----- 5 files changed, 166 insertions(+), 47 deletions(-) diff --git a/src/rtcm3.c b/src/rtcm3.c index 6164917d6..d415378b7 100644 --- a/src/rtcm3.c +++ b/src/rtcm3.c @@ -722,11 +722,54 @@ static int decode_type1012(rtcm_t *rtcm) rtcm->obsflag=!sync; return sync?0:1; } -/* decode type 1013: system parameters ---------------------------------------*/ +// Decode type 1013: system parameters --------------------------------------- static int decode_type1013(rtcm_t *rtcm) { - (void)rtcm; - return 0; + unsigned i = 24 + 12; + if (i + 58 > rtcm->len * 8) { + trace(2,"rtcm3 1013 length error: len=%d\n", rtcm->len); + return -1; + } + + unsigned refid = getbitu(rtcm->buff, i, 12); + i += 12; + unsigned mjd =getbitu(rtcm->buff, i, 16); + i += 16; + unsigned tod = getbitu(rtcm->buff, i, 17); + i += 17; + unsigned nmsg = getbitu(rtcm->buff, i, 5); + i += 5; + unsigned leaps = getbitu(rtcm->buff, i, 8); + i += 8; + + if (i + nmsg * 29 > rtcm->len * 8) { + trace(2,"rtcm3 1013 length error: len=%d nm=%d\n", rtcm->len, nmsg); + return -1; + } + + rtcm->nmsg = nmsg; + uint8_t sync[32]; + for (int n = 0; n < nmsg; n++) { + rtcm->msgs[n] = getbitu(rtcm->buff, i, 12); + i += 12; + sync[n] = getbitu(rtcm->buff, i, 1); + i += 1; + rtcm->tint[n] = getbitu(rtcm->buff, i, 16) * 0.1; + i += 16; + } + + // Convert to GPST. + const double ep[] = {2000, 1, 1, 12, 0, 0}; + gtime_t time = timeadd(epoch2time(ep), (mjd - 51544.5) * 86400.0 + tod + leaps); + + char tstr[40]; + time2str(time, tstr, 0); + if (rtcm->outtype) { + char *msg = rtcm->msgtype + strlen(rtcm->msgtype); + sprintf(msg, " %s refid=%4u mjd=%u tod=%u leaps=%u nmsg=%u", tstr, refid, mjd, tod, leaps, nmsg); + } + + return 0; } /* decode type 1019: GPS ephemerides -----------------------------------------*/ static int decode_type1019(rtcm_t *rtcm) @@ -2598,7 +2641,7 @@ extern int decode_rtcm3(rtcm_t *rtcm) 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 1013: ret=decode_type1013(rtcm); break; case 1019: ret=decode_type1019(rtcm); break; case 1020: ret=decode_type1020(rtcm); break; case 1021: ret=decode_type1021(rtcm); break; /* not supported */ diff --git a/src/rtcm3e.c b/src/rtcm3e.c index 83ca757cc..6f68a4410 100644 --- a/src/rtcm3e.c +++ b/src/rtcm3e.c @@ -788,6 +788,31 @@ static int encode_type1012(rtcm_t *rtcm, int sync) rtcm->nbit=i; return 1; } +// Encode type 1013: system parameters ----------------------------------------- +static int encode_type1013(rtcm_t *rtcm, int sync) +{ + trace(3,"encode_type1013: sync=%d\n", sync); + int i = 24; + const double ep[] = {2000, 1, 1, 12, 0, 0}; + gtime_t utc = gpst2utc(rtcm->time); + int leaps = timediff(rtcm->time, utc); + double mjd = 51544.5 + (timediff(utc, epoch2time(ep))) / 86400.0; + uint32_t mjdi = floor(mjd); + uint32_t tod = round((mjd - mjdi) * 86400.0); + setbitu(rtcm->buff, i, 12, 1013 ); i += 12; // Message no. + setbitu(rtcm->buff, i, 12, 0 ); i += 12; // Ref station id. + setbitu(rtcm->buff, i, 16, mjd ); i += 16; // MJD. + setbitu(rtcm->buff, i, 17, tod ); i += 17; // Time of day, seconds. + setbitu(rtcm->buff, i, 5, rtcm->nmsg); i += 5; // Number of messages. + setbitu(rtcm->buff, i, 8, leaps ); i += 8; // Leap seconds, GPST-UTC. + for (int n = 0; n < rtcm->nmsg; n++) { + setbitu(rtcm->buff, i, 12, rtcm->msgs[n]); i+=12; // Message ID. + setbitu(rtcm->buff, i, 1, 1 ); i+= 1; // Synchronous. + setbitu(rtcm->buff, i, 16, round(rtcm->tint[n] * 10.0)); i+= 16; // Interval. + } + rtcm->nbit=i; + return 1; +} /* encode type 1019: GPS ephemerides -----------------------------------------*/ static int encode_type1019(rtcm_t *rtcm, int sync) { @@ -2665,6 +2690,7 @@ extern int encode_rtcm3(rtcm_t *rtcm, int type, int subtype, int sync) case 1010: ret=encode_type1010(rtcm,sync); break; case 1011: ret=encode_type1011(rtcm,sync); break; case 1012: ret=encode_type1012(rtcm,sync); break; + case 1013: ret=encode_type1013(rtcm,sync); break; case 1019: ret=encode_type1019(rtcm,sync); break; case 1020: ret=encode_type1020(rtcm,sync); break; case 1033: ret=encode_type1033(rtcm,sync); break; diff --git a/src/rtklib.h b/src/rtklib.h index c19433528..41a836467 100644 --- a/src/rtklib.h +++ b/src/rtklib.h @@ -982,6 +982,9 @@ typedef struct { /* RTCM control struct type */ uint32_t nmsg2[100]; /* message count of RTCM 2 (1-99:1-99,0:other) */ uint32_t nmsg3[400]; /* message count of RTCM 3 (1-299:1001-1299,300-329:4070-4099,0:other) */ char opt[256]; /* RTCM dependent options */ + int nmsg; /* number of output messages */ + int msgs[32]; /* output message types */ + double tint[32]; /* output message intervals (s) */ } rtcm_t; typedef struct { /* RINEX control struct type */ @@ -1272,9 +1275,6 @@ typedef struct { /* stream type */ typedef struct { /* stream converter type */ int itype,otype; /* input and output stream type */ - int nmsg; /* number of output messages */ - int msgs[32]; /* output message types */ - double tint[32]; /* output message intervals (s) */ uint32_t tick[32]; /* cycle tick of output message */ int ephsat[32]; /* satellites of output ephemeris */ int stasel; /* station info selection (0:remote,1:local) */ diff --git a/src/streamsvr.c b/src/streamsvr.c index bb1f2bf5e..cdd495e26 100644 --- a/src/streamsvr.c +++ b/src/streamsvr.c @@ -53,7 +53,7 @@ static int is_navmsg(int msg) /* test station info message -------------------------------------------------*/ static int is_stamsg(int msg) { - return msg==1005||msg==1006||msg==1007||msg==1008||msg==1033||msg==1230; + return msg==1005||msg==1006||msg==1007||msg==1008||msg==1013||msg==1033||msg==1230; } /* test time interval --------------------------------------------------------*/ static int is_tint(gtime_t time, double tint) @@ -81,19 +81,19 @@ extern strconv_t *strconvnew(int itype, int otype, const char *msgs, int staid, if (!(conv=(strconv_t *)malloc(sizeof(strconv_t)))) return NULL; - conv->nmsg=0; + conv->out.nmsg=0; strcpy(buff,msgs); char *q; for (p=strtok_r(buff,",",&q);p;p=strtok_r(NULL,",",&q)) { tint=0.0; if (sscanf(p,"%d(%lf)",&msg,&tint)<1) continue; - conv->msgs[conv->nmsg]=msg; - conv->tint[conv->nmsg]=tint; - conv->tick[conv->nmsg]=tickget(); - conv->ephsat[conv->nmsg++]=0; - if (conv->nmsg>=32) break; + conv->out.msgs[conv->out.nmsg]=msg; + conv->out.tint[conv->out.nmsg]=tint; + conv->tick[conv->out.nmsg]=tickget(); + conv->ephsat[conv->out.nmsg++]=0; + if (conv->out.nmsg>=32) break; } - if (conv->nmsg<=0) { + if (conv->out.nmsg<=0) { free(conv); return NULL; } @@ -281,28 +281,28 @@ static void write_obs(gtime_t time, stream_t *str, strconv_t *conv) { int i,j=0; - for (i=0;inmsg;i++) { - if (!is_obsmsg(conv->msgs[i])||!is_tint(time,conv->tint[i])) continue; + for (i=0;iout.nmsg;i++) { + if (!is_obsmsg(conv->out.msgs[i])||!is_tint(time,conv->out.tint[i])) continue; j=i; /* index of last message */ } - for (i=0;inmsg;i++) { - if (!is_obsmsg(conv->msgs[i])||!is_tint(time,conv->tint[i])) continue; + for (i=0;iout.nmsg;i++) { + if (!is_obsmsg(conv->out.msgs[i])||!is_tint(time,conv->out.tint[i])) continue; /* generate messages */ if (conv->otype==STRFMT_RTCM2) { - if (!gen_rtcm2(&conv->out,conv->msgs[i],i!=j)) continue; + if (!gen_rtcm2(&conv->out,conv->out.msgs[i],i!=j)) continue; /* write messages to stream */ strwrite(str,conv->out.buff,conv->out.nbyte); } else if (conv->otype==STRFMT_RTCM3) { - if (conv->msgs[i]<=1012) { - if (!gen_rtcm3(&conv->out,conv->msgs[i],0,i!=j)) continue; + if (conv->out.msgs[i]<=1012) { + if (!gen_rtcm3(&conv->out,conv->out.msgs[i],0,i!=j)) continue; strwrite(str,conv->out.buff,conv->out.nbyte); } else { /* write rtcm3 msm to stream */ - write_rtcm3_msm(str,&conv->out,conv->msgs[i],i!=j); + write_rtcm3_msm(str,&conv->out,conv->out.msgs[i],i!=j); } } } @@ -313,15 +313,15 @@ static void write_nav(gtime_t time, stream_t *str, strconv_t *conv) (void)time; int i; - for (i=0;inmsg;i++) { - if (!is_navmsg(conv->msgs[i])||conv->tint[i]>0.0) continue; + for (i=0;iout.nmsg;i++) { + if (!is_navmsg(conv->out.msgs[i])||conv->out.tint[i]>0.0) continue; /* generate messages */ if (conv->otype==STRFMT_RTCM2) { - if (!gen_rtcm2(&conv->out,conv->msgs[i],0)) continue; + if (!gen_rtcm2(&conv->out,conv->out.msgs[i],0)) continue; } else if (conv->otype==STRFMT_RTCM3) { - if (!gen_rtcm3(&conv->out,conv->msgs[i],0,0)) continue; + if (!gen_rtcm3(&conv->out,conv->out.msgs[i],0,0)) continue; } else continue; @@ -367,26 +367,26 @@ static void write_nav_cycle(stream_t *str, strconv_t *conv) uint32_t tick=tickget(); int i,sat,tint; - for (i=0;inmsg;i++) { - if (!is_navmsg(conv->msgs[i])||conv->tint[i]<=0.0) continue; + for (i=0;iout.nmsg;i++) { + if (!is_navmsg(conv->out.msgs[i])||conv->out.tint[i]<=0.0) continue; /* output cycle */ - tint=(int)(conv->tint[i]*1000.0); + tint=(int)(conv->out.tint[i]*1000.0); if ((int)(tick-conv->tick[i])tick[i]=tick; /* next satellite */ - if (!(sat=nextsat(&conv->out.nav,conv->ephsat[i],conv->msgs[i]))) { + if (!(sat=nextsat(&conv->out.nav,conv->ephsat[i],conv->out.msgs[i]))) { continue; } conv->out.ephsat=conv->ephsat[i]=sat; /* generate messages */ if (conv->otype==STRFMT_RTCM2) { - if (!gen_rtcm2(&conv->out,conv->msgs[i],0)) continue; + if (!gen_rtcm2(&conv->out,conv->out.msgs[i],0)) continue; } else if (conv->otype==STRFMT_RTCM3) { - if (!gen_rtcm3(&conv->out,conv->msgs[i],0,0)) continue; + if (!gen_rtcm3(&conv->out,conv->out.msgs[i],0,0)) continue; } else continue; @@ -399,21 +399,21 @@ static void write_sta_cycle(stream_t *str, strconv_t *conv) { uint32_t tick=tickget(); int i,tint; - - for (i=0;inmsg;i++) { - if (!is_stamsg(conv->msgs[i])) continue; + + for (i=0;iout.nmsg;i++) { + if (!is_stamsg(conv->out.msgs[i])) continue; /* output cycle */ - tint=conv->tint[i]==0.0?30000:(int)(conv->tint[i]*1000.0); + tint=conv->out.tint[i]==0.0?30000:(int)(conv->out.tint[i]*1000.0); if ((int)(tick-conv->tick[i])tick[i]=tick; /* generate messages */ if (conv->otype==STRFMT_RTCM2) { - if (!gen_rtcm2(&conv->out,conv->msgs[i],0)) continue; + if (!gen_rtcm2(&conv->out,conv->out.msgs[i],0)) continue; } else if (conv->otype==STRFMT_RTCM3) { - if (!gen_rtcm3(&conv->out,conv->msgs[i],0,0)) continue; + if (!gen_rtcm3(&conv->out,conv->out.msgs[i],0,0)) continue; } else continue; diff --git a/util/rnx2rtcm/rnx2rtcm.c b/util/rnx2rtcm/rnx2rtcm.c index 07ca123be..075547d57 100644 --- a/util/rnx2rtcm/rnx2rtcm.c +++ b/util/rnx2rtcm/rnx2rtcm.c @@ -106,11 +106,15 @@ static void gen_rtcm_obs(rtcm_t *rtcm, const int *type, int n, FILE *fp) { int j = 0; for (int i = 0; i < n; i++) { - if (is_nav(type[i]) || is_gnav(type[i]) || is_ant(type[i])) continue; + if (is_nav(type[i]) || is_gnav(type[i]) || is_ant(type[i]) || type[i] == 1013 || + type[i] == 1230) + continue; j = i; // Index of last message } for (int i = 0; i < n; i++) { - if (is_nav(type[i]) || is_gnav(type[i]) || is_ant(type[i])) continue; + if (is_nav(type[i]) || is_gnav(type[i]) || is_ant(type[i]) || type[i] == 1013 || + type[i] == 1230) + continue; int msg = type[i]; int sync = i != j; @@ -173,9 +177,26 @@ static void gen_rtcm_ant(rtcm_t *rtcm, const int *type, int n, FILE *fp) { if (fwrite(rtcm->buff, rtcm->nbyte, 1, fp) < 1) break; } } +// Generate RTCM system parameters ------------------------------------------- +static void gen_rtcm_sysparams(rtcm_t *rtcm, const int *type, int n, FILE *fp) { + for (int i = 0; i < n; i++) { + if (type[i] != 1013) continue; + if (!gen_rtcm3(rtcm, type[i], 0, 0)) continue; + if (fwrite(rtcm->buff, rtcm->nbyte, 1, fp) < 1) break; + } +} +// Generate RTCM GLONASS L1 and L2 code-phase biases ------------------------- +static void gen_glo_biases(rtcm_t *rtcm, const int *type, int n, FILE *fp) { + for (int i = 0; i < n; i++) { + if (type[i] != 1230) continue; + if (!gen_rtcm3(rtcm, type[i], 0, 0)) continue; + if (fwrite(rtcm->buff, rtcm->nbyte, 1, fp) < 1) break; + } +} // Convert to RTCM messages -------------------------------------------------- -static int conv_rtcm(const int *type, int n, const char *opt, const char *outfile, - const obs_t *obs, const nav_t *nav, const sta_t *sta, int staid) { +static int conv_rtcm(const int *type, const double *tint, int n, const char *opt, + const char *outfile, const obs_t *obs, const nav_t *nav, const sta_t *sta, + int staid) { rtcm_t rtcm = {0}; strcpy(rtcm.opt, opt); @@ -209,13 +230,26 @@ static int conv_rtcm(const int *type, int n, const char *opt, const char *outfil rtcm.staid = staid; rtcm.sta = *sta; + rtcm.nmsg = n; + for (int i = 0; i < n; i++) { + rtcm.msgs[i] = type[i]; + rtcm.tint[i] = tint[i]; + } + FILE *fp = stdout; if (*outfile && !(fp = fopen(outfile, "wb"))) { fprintf(stderr, "file open error: %s\n", outfile); return 0; } + + rtcm.time = obs->data[0].time; + + // Generate RTCM system paramaters message. + gen_rtcm_sysparams(&rtcm, type, n, fp); // Generate RTCM antenna info messages gen_rtcm_ant(&rtcm, type, n, fp); + // Generate RTCM GLONASS L1 and L2 code-phase biases. + gen_glo_biases(&rtcm, type, n, fp); int index[3] = {0}; int j = -1; @@ -261,8 +295,9 @@ int main(int argc, char **argv) { double es[6] = {0}, ee[6] = {0}, tint = 0.0; char *infile[16] = {0}, *outfile = ""; int n = 0, trlevel = 0, staid = 0; - int type[16], m = 0; + int type[32], m = 0; char *rnxopt = "", *rtcmopt = ""; + double otint[32]; for (int i = 1; i < argc; i++) { if (!strcmp(argv[i], "-ts") && i + 2 < argc) { @@ -281,7 +316,22 @@ int main(int argc, char **argv) { char buff[1024]; strcpy(buff, argv[++i]); char *p; - for (p = strtok(buff, ","); p; p = strtok(NULL, ",")) type[m++] = atoi(p); + for (p = strtok(buff, ","); p && m < 32; p = strtok(NULL, ",")) { + double interval = 0.0; + int msg, r = sscanf(p, "%d(%lf)", &msg, &interval); + if (r < 1) continue; + if (r < 2) { + // Default the interval. + if (is_nav(msg) || is_gnav(msg)) + interval = 60; + else if (is_ant(msg) || msg == 1013 || msg == 1230) + interval = 10; + else + interval = tint; + } + type[m] = msg; + otint[m++] = interval; + } } else if (!strcmp(argv[i], "-opt") && i + 1 < argc) { rtcmopt = argv[++i]; } else if (!strcmp(argv[i], "-rnxopt") && i + 1 < argc) { @@ -309,11 +359,11 @@ int main(int argc, char **argv) { readrnxt(infile[i], 0, ts, te, tint, rnxopt, &obs, &nav, &sta); } sortobs(&obs); - uniqnav(&nav); // To sort them for gen_rtcm_nav() + uniqnav(&nav); // To sort them for gen_rtcm_nav() // Convert to RTCM messages int ret = 0; - if (!conv_rtcm(type, m, rtcmopt, outfile, &obs, &nav, &sta, staid)) ret = -1; + if (!conv_rtcm(type, otint, m, rtcmopt, outfile, &obs, &nav, &sta, staid)) ret = -1; free(obs.data); freenav(&nav, 0xFF);