diff --git a/src/algorithms/libs/rtklib/rtklib_preceph.cc b/src/algorithms/libs/rtklib/rtklib_preceph.cc index 9cbb88509..ff1420ed9 100644 --- a/src/algorithms/libs/rtklib/rtklib_preceph.cc +++ b/src/algorithms/libs/rtklib/rtklib_preceph.cc @@ -494,62 +494,83 @@ int readdcbf(const char *file, nav_t *nav, const sta_t *sta) double cbias; char buff[256]; char str1[32]; - char str2[32] = ""; + char str2[32]; int i; int j; int sat; - int type = 0; + int type = 0; /* 0 = none, 1=P1-P2, 2=P1-C1, 3=P2-C2 */ trace(3, "readdcbf: file=%s\n", file); - if (!(fp = fopen(file, "re"))) + if (!(fp = fopen(file, "r"))) { trace(2, "dcb parameters file open error: %s\n", file); return 0; } + while (fgets(buff, sizeof(buff), fp)) { + /* Detect section headers */ if (strstr(buff, "DIFFERENTIAL (P1-P2) CODE BIASES")) { type = 1; + continue; } else if (strstr(buff, "DIFFERENTIAL (P1-C1) CODE BIASES")) { type = 2; + continue; } else if (strstr(buff, "DIFFERENTIAL (P2-C2) CODE BIASES")) { type = 3; + continue; } - if (!type || sscanf(buff, "%s %s", str1, str2) < 1) + /* Skip lines until we are in a known section */ + if (!type) { continue; } - if ((cbias = str2num(buff, 26, 9)) == 0.0) + /* Parse tokens */ + str1[0] = '\0'; + str2[0] = '\0'; + if (sscanf(buff, "%31s %31s", str1, str2) < 1) { continue; } + /* Parse bias value */ + cbias = str2num(buff, 26, 9); + if (fabs(cbias) < 1e-12) + { + continue; /* skip zero/near-zero biases */ + } + if (sta && (!strcmp(str1, "G") || !strcmp(str1, "R"))) - { /* receiver dcb */ + { + /* Receiver DCB */ for (i = 0; i < MAXRCV; i++) { if (!strcmp(sta[i].name, str2)) { + j = !strcmp(str1, "G") ? 0 : 1; + nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; // ns -> m break; } } - if (i < MAXRCV) - { - j = !strcmp(str1, "G") ? 0 : 1; - nav->rbias[i][j][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */ - } } - else if ((sat = satid2no(str1))) - { /* satellite dcb */ - nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; /* ns -> m */ + else if ((sat = satid2no(str1)) > 0) + { + /* Satellite DCB */ + nav->cbias[sat - 1][type - 1] = cbias * 1e-9 * SPEED_OF_LIGHT_M_S; // ns -> m + } + + /* reset type when a blank line is found (end of section) */ + if (buff[0] == '\n' || buff[0] == '\r') + { + type = 0; } } fclose(fp); diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 1ef558bc5..1ae36e4bc 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -1570,15 +1570,12 @@ void time2epoch(gtime_t t, double *ep) const int mday[] = {/* # of days in a month */ 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 29, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31}; - int days; - int sec; - int mon; - int day; + const auto days = static_cast(t.time / 86400); /* leap year if year%4==0 in 1901-2099 */ + const auto sec = static_cast(t.time - static_cast(days) * 86400); + int day = days % 1461; /* Days in current 4-year cycle */ + int mon = 0; - /* leap year if year%4==0 in 1901-2099 */ - days = static_cast(t.time / 86400); - sec = static_cast(t.time - static_cast(days) * 86400); - for (day = days % 1461, mon = 0; mon < 48; mon++) + for (; mon < 48; mon++) { if (day >= mday[mon]) { @@ -1589,11 +1586,15 @@ void time2epoch(gtime_t t, double *ep) break; } } - ep[0] = 1970 + days / 1461 * 4 + mon / 12; + const int years_4cycle = days / 1461; /* Full 4-year cycles since 1970 */ + const int years_in_cycle = mon / 12; /* Full years in current cycle */ + const int hour = sec / 3600; + const int minutes = sec % 3600 / 60; + ep[0] = 1970 + years_4cycle * 4 + years_in_cycle; ep[1] = mon % 12 + 1; ep[2] = day + 1; - ep[3] = sec / 3600; - ep[4] = sec % 3600 / 60; + ep[3] = static_cast(hour); + ep[4] = static_cast(minutes); ep[5] = sec % 60 + t.sec; } diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index 22e66eadb..ffec14b93 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -67,40 +67,47 @@ void writesol(rtksvr_t *svr, int index) { solopt_t solopt = SOLOPT_DEFAULT; unsigned char buff[MAXSOLMSG]; - int i; int n; tracet(4, "writesol: index=%d\n", index); - for (i = 0; i < 2; i++) + /* Helper lambda to output & save a solution buffer safely */ + auto output_and_save = [&](unsigned char *buf, int len, int stream_id, int sol_index) { + if (len > 0 && len <= MAXSOLMSG) + { + strwrite(svr->stream + stream_id, buf, len); + saveoutbuf(svr, buf, len, sol_index); + } + }; + + for (int i = 0; i < 2; i++) { - /* output solution */ + /* Normal solution */ n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, svr->solopt + i); - strwrite(svr->stream + i + 3, buff, n); + output_and_save(buff, n, i + 3, i); - /* save output buffer */ - saveoutbuf(svr, buff, n, i); - - /* output extended solution */ + /* Extended solution */ n = outsolexs(buff, &svr->rtk.sol, svr->rtk.ssat, svr->solopt + i); - strwrite(svr->stream + i + 3, buff, n); - - /* save output buffer */ - saveoutbuf(svr, buff, n, i); + output_and_save(buff, n, i + 3, i); } - /* output solution to monitor port */ + + /* Output solution to monitor port */ if (svr->moni) { n = outsols(buff, &svr->rtk.sol, svr->rtk.rb, &solopt); - strwrite(svr->moni, buff, n); + if (n > 0 && n <= MAXSOLMSG) + { + strwrite(svr->moni, buff, n); + } } - /* save solution buffer */ + + /* Save solution buffer */ + rtksvrlock(svr); if (svr->nsol < MAXSOLBUF) { - rtksvrlock(svr); svr->solbuf[svr->nsol++] = svr->rtk.sol; - rtksvrunlock(svr); } + rtksvrunlock(svr); } @@ -1055,26 +1062,48 @@ int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs, *-----------------------------------------------------------------------------*/ void rtksvrstop(rtksvr_t *svr, char **cmds) { - int i; + if (!svr || !cmds) + { + tracet(1, "rtksvrstop: Invalid arguments\n"); + return; + } tracet(3, "rtksvrstop:\n"); - /* write stop commands to input streams */ + /* copy stop commands to input streams */ + char *local_cmds[3] = {nullptr, nullptr, nullptr}; rtksvrlock(svr); - for (i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { - if (cmds[i]) - { - strsendcmd(svr->stream + i, cmds[i]); - } + local_cmds[i] = cmds[i]; } rtksvrunlock(svr); - /* stop rtk server */ - svr->state = 0; + /* write stop commands to input streams */ + for (int i = 0; i < 3; i++) + { + if (local_cmds[i]) + { + strsendcmd(svr->stream + i, local_cmds[i]); + } + } - /* free rtk server thread */ - pthread_join(svr->thread, nullptr); + rtksvrlock(svr); + svr->state = 0; + rtksvrunlock(svr); + + if (svr->thread) + { + int ret = pthread_join(svr->thread, nullptr); + if (ret != 0) + { + tracet(1, "rtksvrstop: pthread_join failed with code %d\n", ret); + } + else + { + svr->thread = nullptr; + } + } } diff --git a/src/algorithms/libs/rtklib/rtklib_stream.cc b/src/algorithms/libs/rtklib/rtklib_stream.cc index e2b8a16e9..b6048ac59 100644 --- a/src/algorithms/libs/rtklib/rtklib_stream.cc +++ b/src/algorithms/libs/rtklib/rtklib_stream.cc @@ -380,7 +380,7 @@ file_t *openfile(std::string const &path, int mode, char *msg) /* file options */ - while (not tokens.empty()) + while (!tokens.empty()) { auto tag = tokens.front(); tokens.pop_front(); @@ -512,82 +512,101 @@ int readfile(file_t *file, unsigned char *buff, int nmax, char *msg) { struct timeval tv = {0, 0}; fd_set rs; - unsigned int t; - unsigned int tick; int nr = 0; - size_t fpos; if (!file) { return 0; } + tracet(4, "readfile: fp=%p nmax=%d\n", file->fp, nmax); + /* Input from stdin */ if (file->fp == stdin) { - /* input from stdin */ - std::memset(&rs, 0, sizeof(fd_set)); + FD_ZERO(&rs); FD_SET(0, &rs); - if (!select(1, &rs, nullptr, nullptr, &tv)) + if (select(1, &rs, nullptr, nullptr, &tv) <= 0) { return 0; } - if ((nr = read(0, buff, nmax)) < 0) - { - return 0; - } - return nr; + nr = read(0, buff, nmax); + return (nr > 0) ? nr : 0; } + + /* If there is a .tag file for replay */ if (file->fp_tag) { + unsigned int t; if (file->repmode) - { /* slave */ - t = (tick_master + file->offset); + { + /* slave mode */ + t = tick_master + file->offset; } else - { /* master */ + { + /* master mode */ t = static_cast((tickget() - file->tick) * file->speed + file->start * 1000.0); } + for (;;) - { /* seek file position */ - if (fread(&tick, sizeof(tick), 1, file->fp_tag) < 1 || - fread(&fpos, sizeof(fpos), 1, file->fp_tag) < 1) + { + struct + { + unsigned int tick; + size_t fpos; + } record; + + if (fread(&record, sizeof(record), 1, file->fp_tag) < 1) { - if (fseek(file->fp, 0, SEEK_END) != 0) + if (fseeko(file->fp, 0, SEEK_END) != 0) { trace(1, "fseek error"); } - std::snprintf(msg, MAXSTRPATH, "end"); + if (msg) + { + std::snprintf(msg, MAXSTRPATH, "end"); + } break; } + + /* Time control */ if (file->repmode || file->speed > 0.0) { - if (static_cast(tick - t) < 1) + int64_t diff = static_cast(record.tick) - static_cast(t); + if (diff < 1) { - continue; + continue; // wait until tick is reached } } + if (!file->repmode) { - tick_master = tick; + tick_master = record.tick; } - std::snprintf(msg, MAXSTRPATH, "T%+.1fs", static_cast(tick) < 0 ? 0.0 : static_cast(tick) / 1000.0); - - if (static_cast(fpos - file->fpos) >= nmax) + if (msg) { - if (fseek(file->fp, fpos, SEEK_SET) != 0) + double seconds = record.tick > 0 ? record.tick / 1000.0 : 0.0; + std::snprintf(msg, MAXSTRPATH, "T%+.1fs", seconds); + } + + /* If jump in file position */ + if (static_cast(record.fpos) - static_cast(file->fpos) >= nmax) + { + if (fseeko(file->fp, static_cast(record.fpos), SEEK_SET) != 0) { trace(1, "Error fseek"); } - file->fpos = fpos; + file->fpos = record.fpos; return 0; } - nmax = static_cast(fpos - file->fpos); + + nmax = static_cast(record.fpos - file->fpos); if (file->repmode || file->speed > 0.0) { - if (fseek(file->fp_tag, -static_cast(sizeof(tick) + sizeof(fpos)), SEEK_CUR) != 0) + if (fseeko(file->fp_tag, -static_cast(sizeof(record)), SEEK_CUR) != 0) { trace(1, "Error fseek"); } @@ -595,16 +614,19 @@ int readfile(file_t *file, unsigned char *buff, int nmax, char *msg) break; } } + + /* Read data from main file */ if (nmax > 0) { - nr = fread(buff, 1, nmax, file->fp); + nr = static_cast(fread(buff, 1, nmax, file->fp)); file->fpos += nr; - if (nr <= 0) + if (nr <= 0 && msg) { std::snprintf(msg, MAXSTRPATH, "end"); } } - tracet(5, "readfile: fp=%p \n nr=%d fpos=%u\n", file->fp, nr, file->fpos); + + tracet(5, "readfile: fp=%p nr=%d fpos=%zu\n", file->fp, nr, file->fpos); return nr; } @@ -2245,14 +2267,15 @@ void strinit(stream_t *stream) int stropen(stream_t *stream, int type, int mode, const char *path) { tracet(3, "stropen: type=%d mode=%d path=%s\n", type, mode, path); - + if (!stream || !path) + { + return 0; /* invalid arguments */ + } + strlock(stream); stream->type = type; stream->mode = mode; - if (strlen(path) < MAXSTRPATH) - { - std::strncpy(stream->path, path, MAXSTRPATH - 1); - stream->path[MAXSTRPATH - 1] = '\0'; - } + std::strncpy(stream->path, path, MAXSTRPATH - 1); + stream->path[MAXSTRPATH - 1] = '\0'; stream->inb = stream->inr = stream->outb = stream->outr = 0; stream->tick = tickget(); stream->inbt = stream->outbt = 0; @@ -2288,8 +2311,9 @@ int stropen(stream_t *stream, int type, int mode, const char *path) stream->state = 0; return 1; } - stream->state = !stream->port ? -1 : 1; - return stream->port != nullptr; + stream->state = (stream->port != nullptr) ? 1 : -1; + strunlock(stream); + return (stream->port != nullptr) ? 1 : 0; }