/*------------------------------------------------------------------------------ * solution.c : solution functions * * Copyright (C) 2007-2020 by T.TAKASU, All rights reserved. * * references : * [1] National Marine Electronic Association and International Marine * Electronics Association, NMEA 0183 version 4.10, August 1, 2012 * [2] NMEA 0183 Talker Identifier Mnemonics, March 3, 2019 * (https://www.nmea.org/content/STANDARDS/NMEA_0183_Standard) * * version : $Revision:$ $Date:$ * history : 2007/11/03 1.0 new * 2009/01/05 1.1 add function outsols(), outsolheads(), * setsolformat(), outsolexs, outsolex * 2009/04/02 1.2 add dummy fields in NMEA mesassage * fix bug to format lat/lon as deg-min-sec * 2009/04/14 1.3 add age and ratio field to solution * 2009/11/25 1.4 add function readsolstat() * 2010/02/14 1.5 fix bug on output of gpstime at week boundary * 2010/07/05 1.6 added api: * initsolbuf(),freesolbuf(),addsol(),getsol(), * inputsol(),outprcopts(),outprcopt() * modified api: * readsol(),readsolt(),readsolstat(), * readsolstatt(),outsolheads(),outsols(), * outsolexs(),outsolhead(),outsol(),outsolex(), * outnmea_rmc(),outnmea_gga(),outnmea_gsa(), * outnmea_gsv() * deleted api: * setsolopt(),setsolformat() * 2010/08/14 1.7 fix bug on initialize solution buffer (2.4.0_p2) * suppress enu-solution if base pos not available * (2.4.0_p3) * 2010/08/16 1.8 suppress null record if solution is not available * (2.4.0_p4) * 2011/01/23 1.9 fix bug on reading nmea solution data * add api freesolstatbuf() * 2012/02/05 1.10 fix bug on output nmea gpgsv * 2013/02/18 1.11 support nmea GLGSA,GAGSA,GLCSV,GACSV sentence * 2013/09/01 1.12 fix bug on presentation of nmea time tag * 2015/02/11 1.13 fix bug on checksum of $GLGSA and $GAGSA * fix bug on satellite id of $GAGSA * 2016/01/17 1.14 support reading NMEA GxZDA * ignore NMEA talker ID * 2016/07/30 1.15 suppress output if std is over opt->maxsolstd * 2017/06/13 1.16 support output/input of velocity solution * 2018/10/10 1.17 support reading solution status file * 2020/11/30 1.18 add NMEA talker ID GQ and GI (NMEA 0183 4.11) * add NMEA GQ/GB/GI-GSA/GSV sentences * change talker ID GP to GN for NMEA RMC/GGA * change newline to "\r\n" in SOLF_LLH,XYZ,ENU * add reading age information in NMEA GGA * use integer types in stdint.h * suppress warnings *-----------------------------------------------------------------------------*/ #include #include "rtklib.h" /* constants and macros ------------------------------------------------------*/ #define SQR(x) ((x) < 0.0 ? -(x) * (x) : (x) * (x)) #define SQRT(x) ((x) < 0.0 || (x) != (x) ? 0.0 : sqrt(x)) /* https://gpsd.gitlab.io/gpsd/NMEA.html#_talker_ids says ... GA ~ Galileo Positioning System GB ~ BeiDou(China) GI ~ NavIC, IRNSS(India) GL ~ GLONASS, according to IEIC 61162 - 1 GN ~ Combination of multiple satellite systems(NMEA 1083) GP ~ Global Positioning System receiver GQ ~ QZSS regional GPS augmentation system(Japan) https://talk.newagtalk.com/forums/thread-view.asp?tid=877179&mid=7732209#M7732209 Says "The industry pretty much standardized on using GPxxx regardless of what constellations are being used in the solution. To be completely NMEA compliant the second letter should correspond to the constellations being used. Don't forget Galileo and Beidou have their own unique letters too." And rnx2rtkp GNxxx output is not plotted by RTKPLOT, whereas GPxxx output is So, This NMEA Talker ID used to be "GN" but now its "GP" */ #define NMEA_TID "GP" /* NMEA talker ID for RMC and GGA sentences */ #define MAXFIELD 64 /* max number of fields in a record */ #define MAXNMEA 256 /* max length of nmea sentence */ #define KNOT2M 0.514444444 /* m/sec --> knot */ static const int nmea_sys[] = {/* NMEA systems */ SYS_GPS | SYS_SBS, SYS_GLO, SYS_GAL, SYS_CMP, SYS_QZS, SYS_IRN, 0}; static const char *nmea_tid[] = {/* NMEA talker IDs [2] */ "GP", "GL", "GA", "GB", "GQ", "GI", ""}; static const int nmea_sid[] = {/* NMEA system IDs [1] table 21 */ 1, 2, 3, 4, 5, 6, 0}; static const int nmea_solq[] = {/* NMEA GPS quality indicator [1] */ /* 0=Fix not available or invalidi */ /* 1=GPS SPS Mode, fix valid */ /* 2=Differential GPS, SPS Mode, fix valid */ /* 3=GPS PPS Mode, fix valid */ /* 4=Real Time Kinematic. System used in RTK mode with fixed integers */ /* 5=Float RTK. Satellite system used in RTK mode, floating integers */ /* 6=Estimated (dead reckoning) Mode */ /* 7=Manual Input Mode */ /* 8=Simulation Mode */ SOLQ_NONE, SOLQ_SINGLE, SOLQ_DGPS, SOLQ_PPP, SOLQ_FIX, SOLQ_FLOAT, SOLQ_DR, SOLQ_NONE, SOLQ_NONE, SOLQ_NONE}; // void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax); // void freesolbuf(solbuf_t *solbuf); // void freesolstatbuf(solstatbuf_t *solstatbuf); // sol_t *getsol(solbuf_t *solbuf, int index); /* solution option to field separator ----------------------------------------*/ static const char *opt2sep(const solopt_t *opt) { if (!*opt->sep) return " "; else if (!strcmp(opt->sep, "\\t")) return "\t"; return opt->sep; } /* separate fields -----------------------------------------------------------*/ static int tonum(char *buff, const char *sep, double *v) { int n, len = (int)strlen(sep); char *p, *q; for (p = buff, n = 0; n < MAXFIELD; p = q + len) { if ((q = strstr(p, sep))) *q = '\0'; if (*p) v[n++] = atof(p); if (!q) break; } return n; } /* sqrt of covariance --------------------------------------------------------*/ static double sqvar(double covar) { return covar < 0.0 ? -sqrt(-covar) : sqrt(covar); } /* convert ddmm.mm in nmea format to deg -------------------------------------*/ static double dmm2deg(double dmm) { return floor(dmm / 100.0) + fmod(dmm, 100.0) / 60.0; } /* convert time in nmea format to time ---------------------------------------*/ static void septime(double t, double *t1, double *t2, double *t3) { *t1 = floor(t / 10000.0); t -= *t1 * 10000.0; *t2 = floor(t / 100.0); *t3 = t - *t2 * 100.0; } /* solution to covariance ----------------------------------------------------*/ static void soltocov(const sol_t *sol, double *P) { P[0] = sol->qr[0]; /* xx or ee */ P[4] = sol->qr[1]; /* yy or nn */ P[8] = sol->qr[2]; /* zz or uu */ P[1] = P[3] = sol->qr[3]; /* xy or en */ P[5] = P[7] = sol->qr[4]; /* yz or nu */ P[2] = P[6] = sol->qr[5]; /* zx or ue */ } /* covariance to solution ----------------------------------------------------*/ static void covtosol(const double *P, sol_t *sol) { sol->qr[0] = (float)P[0]; /* xx or ee */ sol->qr[1] = (float)P[4]; /* yy or nn */ sol->qr[2] = (float)P[8]; /* zz or uu */ sol->qr[3] = (float)P[1]; /* xy or en */ sol->qr[4] = (float)P[5]; /* yz or nu */ sol->qr[5] = (float)P[2]; /* zx or ue */ } /* solution to velocity covariance -------------------------------------------*/ static void soltocov_vel(const sol_t *sol, double *P) { P[0] = sol->qv[0]; /* xx */ P[4] = sol->qv[1]; /* yy */ P[8] = sol->qv[2]; /* zz */ P[1] = P[3] = sol->qv[3]; /* xy */ P[5] = P[7] = sol->qv[4]; /* yz */ P[2] = P[6] = sol->qv[5]; /* zx */ } /* velocity covariance to solution -------------------------------------------*/ static void covtosol_vel(const double *P, sol_t *sol) { sol->qv[0] = (float)P[0]; /* xx */ sol->qv[1] = (float)P[4]; /* yy */ sol->qv[2] = (float)P[8]; /* zz */ sol->qv[3] = (float)P[1]; /* xy */ sol->qv[4] = (float)P[5]; /* yz */ sol->qv[5] = (float)P[2]; /* zx */ } /* decode NMEA RMC (Recommended Minumum Specific GNSS Data) sentence ---------*/ static int decode_nmearmc(char **val, int n, sol_t *sol) { double tod = 0.0, lat = 0.0, lon = 0.0, vel = 0.0, dir = 0.0, date = 0.0, ang = 0.0, ep[6]; double pos[3] = {0}; char act = ' ', ns = 'N', ew = 'E', mew = 'E', mode = 'A'; int i; trace(4, "decode_nmearmc: n=%d\n", n); for (i = 0; i < n; i++) { switch (i) { case 0: tod = atof(val[i]); break; /* time in utc (hhmmss) */ case 1: act = *val[i]; break; /* A=active,V=void */ case 2: lat = atof(val[i]); break; /* latitude (ddmm.mmm) */ case 3: ns = *val[i]; break; /* N=north,S=south */ case 4: lon = atof(val[i]); break; /* longitude (dddmm.mmm) */ case 5: ew = *val[i]; break; /* E=east,W=west */ case 6: vel = atof(val[i]); break; /* speed (knots) */ case 7: dir = atof(val[i]); break; /* track angle (deg) */ case 8: date = atof(val[i]); break; /* date (ddmmyy) */ case 9: ang = atof(val[i]); break; /* magnetic variation */ case 10: mew = *val[i]; break; /* E=east,W=west */ case 11: mode = *val[i]; break; /* mode indicator (>nmea 2) */ /* A=autonomous,D=differential */ /* E=estimated,N=not valid,S=simulator */ } } if ((act != 'A' && act != 'V') || (ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W')) { trace(3, "invalid nmea rmc format\n"); return 0; } pos[0] = (ns == 'S' ? -1.0 : 1.0) * dmm2deg(lat) * D2R; pos[1] = (ew == 'W' ? -1.0 : 1.0) * dmm2deg(lon) * D2R; septime(date, ep + 2, ep + 1, ep); septime(tod, ep + 3, ep + 4, ep + 5); ep[0] += ep[0] < 80.0 ? 2000.0 : 1900.0; sol->time = utc2gpst(epoch2time(ep)); pos2ecef(pos, sol->rr); sol->stat = mode == 'D' ? SOLQ_DGPS : SOLQ_SINGLE; sol->ns = 0; sol->type = 0; /* postion type = xyz */ trace(5, "decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n", time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns, vel, dir, ang, mew, mode); return 2; /* update time */ } /* decode NMEA ZDA (Time and Date) sentence ----------------------------------*/ static int decode_nmeazda(char **val, int n, sol_t *sol) { double tod = 0.0, ep[6] = {0}; int i; trace(4, "decode_nmeazda: n=%d\n", n); for (i = 0; i < n; i++) { switch (i) { case 0: tod = atof(val[i]); break; /* time in utc (hhmmss) */ case 1: ep[2] = atof(val[i]); break; /* day (0-31) */ case 2: ep[1] = atof(val[i]); break; /* mon (1-12) */ case 3: ep[0] = atof(val[i]); break; /* year */ } } septime(tod, ep + 3, ep + 4, ep + 5); sol->time = utc2gpst(epoch2time(ep)); sol->ns = 0; trace(5, "decode_nmeazda: %s\n", time_str(sol->time, 0)); return 2; /* update time */ } /* decode NMEA GGA (Global Positioning System Fix Data) sentence -------------*/ static int decode_nmeagga(char **val, int n, sol_t *sol) { gtime_t time; double tod = 0.0, lat = 0.0, lon = 0.0, hdop = 0.0, alt = 0.0, msl = 0.0, ep[6], tt; double pos[3] = {0}, age = 0.0; char ns = 'N', ew = 'E', ua = ' ', um = ' '; int i, solq = 0, nrcv = 0; trace(4, "decode_nmeagga: n=%d\n", n); for (i = 0; i < n; i++) { switch (i) { case 0: tod = atof(val[i]); break; /* UTC of position (hhmmss) */ case 1: lat = atof(val[i]); break; /* Latitude (ddmm.mmm) */ case 2: ns = *val[i]; break; /* N=north,S=south */ case 3: lon = atof(val[i]); break; /* Longitude (dddmm.mmm) */ case 4: ew = *val[i]; break; /* E=east,W=west */ case 5: solq = atoi(val[i]); break; /* GPS quality indicator */ case 6: nrcv = atoi(val[i]); break; /* # of satellites in use */ case 7: hdop = atof(val[i]); break; /* HDOP */ case 8: alt = atof(val[i]); break; /* Altitude MSL */ case 9: ua = *val[i]; break; /* unit (M) */ case 10: msl = atof(val[i]); break; /* Geoid separation */ case 11: um = *val[i]; break; /* unit (M) */ case 12: age = atof(val[i]); break; /* Age of differential */ } } if ((ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W')) { trace(3, "invalid nmea gga format\n"); return 0; } if (sol->time.time == 0) { trace(3, "no date info for nmea gga\n"); return 0; } pos[0] = (ns == 'N' ? 1.0 : -1.0) * dmm2deg(lat) * D2R; pos[1] = (ew == 'E' ? 1.0 : -1.0) * dmm2deg(lon) * D2R; pos[2] = alt + msl; time2epoch(sol->time, ep); septime(tod, ep + 3, ep + 4, ep + 5); time = utc2gpst(epoch2time(ep)); tt = timediff(time, sol->time); if (tt < -43200.0) sol->time = timeadd(time, 86400.0); else if (tt > 43200.0) sol->time = timeadd(time, -86400.0); else sol->time = time; pos2ecef(pos, sol->rr); sol->stat = 0 <= solq && solq <= 8 ? nmea_solq[solq] : SOLQ_NONE; sol->ns = nrcv; sol->age = (float)age; sol->type = 0; /* postion type = xyz */ trace(5, "decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n", time_str(sol->time, 0), sol->rr[0], sol->rr[1], sol->rr[2], sol->stat, sol->ns, hdop, ua, um); return 1; } /* test NMEA sentence header -------------------------------------------------*/ static int test_nmea(const char *buff) { if (strlen(buff) < 6 || buff[0] != '$') return 0; return !strncmp(buff + 1, "GP", 2) || !strncmp(buff + 1, "GA", 2) || /* NMEA 4.10 [1] */ !strncmp(buff + 1, "GL", 2) || !strncmp(buff + 1, "GN", 2) || !strncmp(buff + 1, "GB", 2) || !strncmp(buff + 1, "GQ", 2) || /* NMEA 4.11 [2] */ !strncmp(buff + 1, "GI", 2) || !strncmp(buff + 1, "BD", 2) || !strncmp(buff + 1, "QZ", 2); /* extension */ } /* test solution status message header ---------------------------------------*/ static int test_solstat(const char *buff) { if (strlen(buff) < 7 || buff[0] != '$') return 0; return !strncmp(buff + 1, "POS", 3) || !strncmp(buff + 1, "VELACC", 6) || !strncmp(buff + 1, "CLK", 3) || !strncmp(buff + 1, "ION", 3) || !strncmp(buff + 1, "TROP", 4) || !strncmp(buff + 1, "HWBIAS", 6) || !strncmp(buff + 1, "TRPG", 4) || !strncmp(buff + 1, "AMB", 3) || !strncmp(buff + 1, "SAT", 3); } /* decode NMEA sentence ------------------------------------------------------*/ static int decode_nmea(char *buff, sol_t *sol) { char *p, *q, *val[MAXFIELD]; int n = 0; trace(4, "decode_nmea: buff=%s\n", buff); /* parse fields */ for (p = buff; *p && n < MAXFIELD; p = q + 1) { if ((q = strchr(p, ',')) || (q = strchr(p, '*'))) { val[n++] = p; *q = '\0'; } else break; } if (n < 1) { return 0; } if (!strcmp(val[0] + 3, "RMC")) { /* $xxRMC */ return decode_nmearmc(val + 1, n - 1, sol); } else if (!strcmp(val[0] + 3, "ZDA")) { /* $xxZDA */ return decode_nmeazda(val + 1, n - 1, sol); } else if (!strcmp(val[0] + 3, "GGA")) { /* $xxGGA */ return decode_nmeagga(val + 1, n - 1, sol); } return 0; } /* decode solution time ------------------------------------------------------*/ static char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time) { double v[MAXFIELD]; char *p, *q, s[64] = " "; int n, len; trace(4, "decode_soltime:\n"); if (!strcmp(opt->sep, "\\t")) strcpy(s, "\t"); else if (*opt->sep) strcpy(s, opt->sep); len = (int)strlen(s); if (opt->posf == SOLF_STAT) { return buff; } if (opt->posf == SOLF_GSIF) { if (sscanf(buff, "%lf %lf %lf %lf:%lf:%lf", v, v + 1, v + 2, v + 3, v + 4, v + 5) < 6) { return NULL; } *time = timeadd(epoch2time(v), -12.0 * 3600.0); if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':'))) return NULL; for (p++; isdigit((int)*p) || *p == '.';) p++; return p + len; } /* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */ if (sscanf(buff, "%lf/%lf/%lf %lf:%lf:%lf", v, v + 1, v + 2, v + 3, v + 4, v + 5) >= 6) { if (v[0] < 100.0) { v[0] += v[0] < 80.0 ? 2000.0 : 1900.0; } *time = epoch2time(v); if (opt->times == TIMES_UTC) { *time = utc2gpst(*time); } else if (opt->times == TIMES_JST) { *time = utc2gpst(timeadd(*time, -9 * 3600.0)); } if (!(p = strchr(buff, ':')) || !(p = strchr(p + 1, ':'))) return NULL; for (p++; isdigit((int)*p) || *p == '.';) p++; return p + len; } else { /* wwww ssss */ for (p = buff, n = 0; n < 2; p = q + len) { if ((q = strstr(p, s))) *q = '\0'; if (sscanf(p, "%lf", v + n) == 1) n++; if (!q) break; } if (n >= 2 && 0.0 <= v[0] && v[0] <= 3000.0 && 0.0 <= v[1] && v[1] < 604800.0) { *time = gpst2time((int)v[0], v[1]); return p; } } return NULL; } /* decode x/y/z-ecef ---------------------------------------------------------*/ static int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD], P[9] = {0}; int i = 0, j, n; const char *sep = opt2sep(opt); trace(4, "decode_solxyz:\n"); if ((n = tonum(buff, sep, val)) < 3) return 0; for (j = 0; j < 3; j++) { sol->rr[j] = val[i++]; /* xyz */ } if (i < n) sol->stat = (uint8_t)val[i++]; if (i < n) sol->ns = (uint8_t)val[i++]; if (i + 3 <= n) { P[0] = SQR(val[i]); i++; /* sdx */ P[4] = SQR(val[i]); i++; /* sdy */ P[8] = SQR(val[i]); i++; /* sdz */ if (i + 3 <= n) { P[1] = P[3] = SQR(val[i]); i++; /* sdxy */ P[5] = P[7] = SQR(val[i]); i++; /* sdyz */ P[2] = P[6] = SQR(val[i]); i++; /* sdzx */ } covtosol(P, sol); } if (i < n) sol->age = (float)val[i++]; if (i < n) sol->ratio = (float)val[i++]; if (i + 3 <= n) { /* velocity */ for (j = 0; j < 3; j++) { sol->rr[j + 3] = val[i++]; /* xyz */ } } if (i + 3 <= n) { for (j = 0; j < 9; j++) P[j] = 0.0; P[0] = SQR(val[i]); i++; /* sdx */ P[4] = SQR(val[i]); i++; /* sdy */ P[8] = SQR(val[i]); i++; /* sdz */ if (i + 3 < n) { P[1] = P[3] = SQR(val[i]); i++; /* sdxy */ P[5] = P[7] = SQR(val[i]); i++; /* sdyz */ P[2] = P[6] = SQR(val[i]); i++; /* sdzx */ } covtosol_vel(P, sol); } sol->type = 0; /* postion type = xyz */ if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; return 1; } /* decode lat/lon/height -----------------------------------------------------*/ static int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD], pos[3], vel[3], Q[9] = {0}, P[9]; int i = 0, j, n; const char *sep = opt2sep(opt); trace(4, "decode_solllh:\n"); n = tonum(buff, sep, val); if (!opt->degf) { if (n < 3) return 0; pos[0] = val[i++] * D2R; /* lat/lon/hgt (ddd.ddd) */ pos[1] = val[i++] * D2R; pos[2] = val[i++]; } else { if (n < 7) return 0; pos[0] = dms2deg(val) * D2R; /* lat/lon/hgt (ddd mm ss) */ pos[1] = dms2deg(val + 3) * D2R; pos[2] = val[6]; i += 7; } pos2ecef(pos, sol->rr); if (i < n) sol->stat = (uint8_t)val[i++]; if (i < n) sol->ns = (uint8_t)val[i++]; if (i + 3 <= n) { Q[4] = SQR(val[i]); i++; /* sdn */ Q[0] = SQR(val[i]); i++; /* sde */ Q[8] = SQR(val[i]); i++; /* sdu */ if (i + 3 < n) { Q[1] = Q[3] = SQR(val[i]); i++; /* sdne */ Q[2] = Q[6] = SQR(val[i]); i++; /* sdeu */ Q[5] = Q[7] = SQR(val[i]); i++; /* sdun */ } covecef(pos, Q, P); covtosol(P, sol); } if (i < n) sol->age = (float)val[i++]; if (i < n) sol->ratio = (float)val[i++]; if (i + 3 <= n) { /* velocity */ vel[1] = val[i++]; /* vel-n */ vel[0] = val[i++]; /* vel-e */ vel[2] = val[i++]; /* vel-u */ enu2ecef(pos, vel, sol->rr + 3); } if (i + 3 <= n) { for (j = 0; j < 9; j++) Q[j] = 0.0; Q[4] = SQR(val[i]); i++; /* sdn */ Q[0] = SQR(val[i]); i++; /* sde */ Q[8] = SQR(val[i]); i++; /* sdu */ if (i + 3 <= n) { Q[1] = Q[3] = SQR(val[i]); i++; /* sdne */ Q[2] = Q[6] = SQR(val[i]); i++; /* sdeu */ Q[5] = Q[7] = SQR(val[i]); i++; /* sdun */ } covecef(pos, Q, P); covtosol_vel(P, sol); } sol->type = 0; /* postion type = xyz */ if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; return 1; } /* decode e/n/u-baseline -----------------------------------------------------*/ static int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD], Q[9] = {0}; int i = 0, j, n; const char *sep = opt2sep(opt); trace(4, "decode_solenu:\n"); if ((n = tonum(buff, sep, val)) < 3) return 0; for (j = 0; j < 3; j++) { sol->rr[j] = val[i++]; /* enu */ } if (i < n) sol->stat = (uint8_t)val[i++]; if (i < n) sol->ns = (uint8_t)val[i++]; if (i + 3 <= n) { Q[0] = SQR(val[i]); i++; /* sde */ Q[4] = SQR(val[i]); i++; /* sdn */ Q[8] = SQR(val[i]); i++; /* sdu */ if (i + 3 <= n) { Q[1] = Q[3] = SQR(val[i]); i++; /* sden */ Q[5] = Q[7] = SQR(val[i]); i++; /* sdnu */ Q[2] = Q[6] = SQR(val[i]); i++; /* sdue */ } covtosol(Q, sol); } if (i < n) sol->age = (float)val[i++]; if (i < n) sol->ratio = (float)val[i++]; if (i + 3 <= n) { /* velocity */ for (j = 0; j < 3; j++) { sol->rr[j + 3] = val[i++]; /* vel-enu */ } } if (i + 3 <= n) { for (j = 0; j < 9; j++) Q[j] = 0.0; Q[0] = val[i] * val[i]; i++; /* sde */ Q[4] = val[i] * val[i]; i++; /* sdn */ Q[8] = val[i] * val[i]; i++; /* sdu */ if (i + 3 <= n) { Q[1] = Q[3] = SQR(val[i]); i++; /* sden */ Q[5] = Q[7] = SQR(val[i]); i++; /* sdnu */ Q[2] = Q[6] = SQR(val[i]); i++; /* sdue */ } covtosol_vel(Q, sol); } sol->type = 1; /* postion type = enu */ if (MAXSOLQ < sol->stat) sol->stat = SOLQ_NONE; return 1; } /* decode solution status ----------------------------------------------------*/ static int decode_solsss(char *buff, sol_t *sol) { double tow, pos[3], std[3] = {0}; int i, week, solq; trace(4, "decode_solsss:\n"); if (sscanf(buff, "$POS,%d,%lf,%d,%lf,%lf,%lf,%lf,%lf,%lf", &week, &tow, &solq, pos, pos + 1, pos + 2, std, std + 1, std + 2) < 6) { return 0; } if (week <= 0 || norm(pos, 3) <= 0.0 || solq == SOLQ_NONE) { return 0; } sol->time = gpst2time(week, tow); for (i = 0; i < 6; i++) { sol->rr[i] = i < 3 ? pos[i] : 0.0; sol->qr[i] = i < 3 ? (float)SQR(std[i]) : 0.0f; sol->dtr[i] = 0.0; } sol->ns = 0; sol->age = sol->ratio = sol->thres = 0.0f; sol->type = 0; /* position type = xyz */ sol->stat = solq; return 1; } /* decode GSI F solution -----------------------------------------------------*/ static int decode_solgsi(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD]; int i = 0, j; trace(4, "decode_solgsi:\n"); if (tonum(buff, " ", val) < 3) return 0; for (j = 0; j < 3; j++) { sol->rr[j] = val[i++]; /* xyz */ } sol->stat = SOLQ_FIX; return 1; } /* decode solution position --------------------------------------------------*/ static int decode_solpos(char *buff, const solopt_t *opt, sol_t *sol) { sol_t sol0 = {{0}}; char *p = buff; trace(4, "decode_solpos: buff=%s\n", buff); *sol = sol0; /* decode solution time */ if (!(p = decode_soltime(p, opt, &sol->time))) { return 0; } /* decode solution position */ switch (opt->posf) { case SOLF_XYZ: return decode_solxyz(p, opt, sol); case SOLF_LLH: return decode_solllh(p, opt, sol); case SOLF_ENU: return decode_solenu(p, opt, sol); case SOLF_GSIF: return decode_solgsi(p, opt, sol); } return 0; } /* decode reference position -------------------------------------------------*/ static void decode_refpos(char *buff, const solopt_t *opt, double *rb) { double val[MAXFIELD], pos[3]; int i, n; const char *sep = opt2sep(opt); trace(3, "decode_refpos: buff=%s\n", buff); if ((n = tonum(buff, sep, val)) < 3) return; if (opt->posf == SOLF_XYZ) { /* xyz */ for (i = 0; i < 3; i++) rb[i] = val[i]; } else if (opt->degf == 0) { /* lat/lon/hgt (ddd.ddd) */ pos[0] = val[0] * D2R; pos[1] = val[1] * D2R; pos[2] = val[2]; pos2ecef(pos, rb); } else if (opt->degf == 1 && n >= 7) { /* lat/lon/hgt (ddd mm ss) */ pos[0] = dms2deg(val) * D2R; pos[1] = dms2deg(val + 3) * D2R; pos[2] = val[6]; pos2ecef(pos, rb); } } /* decode solution -----------------------------------------------------------*/ static int decode_sol(char *buff, const solopt_t *opt, sol_t *sol, double *rb) { char *p; trace(4, "decode_sol: buff=%s\n", buff); if (test_nmea(buff)) { /* decode nmea */ return decode_nmea(buff, sol); } else if (test_solstat(buff)) { /* decode solution status */ return decode_solsss(buff, sol); } if (!strncmp(buff, COMMENTH, 1)) { /* reference position */ if (!strstr(buff, "ref pos") && !strstr(buff, "slave pos")) return 0; if (!(p = strchr(buff, ':'))) return 0; decode_refpos(p + 1, opt, rb); return 0; } /* decode position record */ return decode_solpos(buff, opt, sol); } /* decode solution options ---------------------------------------------------*/ static void decode_solopt(char *buff, solopt_t *opt) { char *p; trace(4, "decode_solhead: buff=%s\n", buff); if (strncmp(buff, COMMENTH, 1) && strncmp(buff, "+", 1)) return; if (strstr(buff, "GPST")) opt->times = TIMES_GPST; else if (strstr(buff, "UTC")) opt->times = TIMES_UTC; else if (strstr(buff, "JST")) opt->times = TIMES_JST; if ((p = strstr(buff, "x-ecef(m)"))) { opt->posf = SOLF_XYZ; opt->degf = 0; strncpy(opt->sep, p + 9, 1); opt->sep[1] = '\0'; } else if ((p = strstr(buff, "latitude(d'\")"))) { opt->posf = SOLF_LLH; opt->degf = 1; strncpy(opt->sep, p + 14, 1); opt->sep[1] = '\0'; } else if ((p = strstr(buff, "latitude(deg)"))) { opt->posf = SOLF_LLH; opt->degf = 0; strncpy(opt->sep, p + 13, 1); opt->sep[1] = '\0'; } else if ((p = strstr(buff, "e-baseline(m)"))) { opt->posf = SOLF_ENU; opt->degf = 0; strncpy(opt->sep, p + 13, 1); opt->sep[1] = '\0'; } else if ((p = strstr(buff, "+SITE/INF"))) { /* gsi f2/f3 solution */ opt->times = TIMES_GPST; opt->posf = SOLF_GSIF; opt->degf = 0; strcpy(opt->sep, " "); } } /* read solution option ------------------------------------------------------*/ static void readsolopt(FILE *fp, solopt_t *opt) { char buff[MAXSOLMSG + 1]; int i; trace(3, "readsolopt:\n"); for (i = 0; fgets(buff, sizeof(buff), fp) && i < 100; i++) { /* only 100 lines */ /* decode solution options */ decode_solopt(buff, opt); } } /* input solution data from stream --------------------------------------------- * input solution data from stream * args : uint8_t data I stream data * gtime_t ts I start time (ts.time==0: from start) * gtime_t te I end time (te.time==0: to end) * double tint I time interval (0: all) * int qflag I quality flag (0: all) * solbuf_t *solbuf IO solution buffer * return : status (1:solution received,0:no solution,-1:disconnect received) *-----------------------------------------------------------------------------*/ // extern int inputsol(uint8_t data, gtime_t ts, gtime_t te, double tint, // int qflag, const solopt_t *opt, solbuf_t *solbuf) //{ // sol_t sol={{0}}; // int stat; // // trace(4,"inputsol: data=0x%02x\n",data); // // sol.time=solbuf->time; // // if (data=='$'||(!isprint(data)&&data!='\r'&&data!='\n')) { /* sync header */ // solbuf->nb=0; // } // if (data!='\r'&&data!='\n') { // solbuf->buff[solbuf->nb++]=data; // } // if (data!='\n'&&solbuf->nbbuff[solbuf->nb]='\0'; // solbuf->nb=0; // // /* check disconnect message */ // if (!strncmp((char *)solbuf->buff,MSG_DISCONN,strlen(MSG_DISCONN)-2)) { // trace(3,"disconnect received\n"); // return -1; // } // /* decode solution */ // sol.time=solbuf->time; // if ((stat=decode_sol((char *)solbuf->buff,opt,&sol,solbuf->rb))>0) { // if (stat) solbuf->time=sol.time; /* update current time */ // if (stat!=1) return 0; // } // if (stat!=1||!screent(sol.time,ts,te,tint)||(qflag&&sol.stat!=qflag)) { // return 0; // } // /* add solution to solution buffer */ // return addsol(solbuf,&sol); // } /* read solution data --------------------------------------------------------*/ // static int readsoldata(FILE *fp, gtime_t ts, gtime_t te, double tint, int qflag, // const solopt_t *opt, solbuf_t *solbuf) //{ // int c; // // trace(3,"readsoldata:\n"); // // while ((c=fgetc(fp))!=EOF) { // // /* input solution */ // inputsol((uint8_t)c,ts,te,tint,qflag,opt,solbuf); // } // return solbuf->n>0; // } /* compare solution data -----------------------------------------------------*/ static int cmpsol(const void *p1, const void *p2) { sol_t *q1 = (sol_t *)p1, *q2 = (sol_t *)p2; double tt = timediff(q1->time, q2->time); return tt < -0.0 ? -1 : (tt > 0.0 ? 1 : 0); } /* sort solution data --------------------------------------------------------*/ // static int sort_solbuf(solbuf_t *solbuf) //{ // sol_t *solbuf_data; // // trace(4,"sort_solbuf: n=%d\n",solbuf->n); // // if (solbuf->n<=0) return 0; // // if (!(solbuf_data=(sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->n))) { // trace(1,"sort_solbuf: memory allocation error\n"); // free(solbuf->data); solbuf->data=NULL; solbuf->n=solbuf->nmax=0; // return 0; // } // solbuf->data=solbuf_data; // qsort(solbuf->data,solbuf->n,sizeof(sol_t),cmpsol); // solbuf->nmax=solbuf->n; // solbuf->start=0; // solbuf->end=solbuf->n-1; // return 1; // } /* read solutions data from solution files ------------------------------------- * read solution data from soluiton files * args : char *files[] I solution files * int nfile I number of files * (gtime_t ts) I start time (ts.time==0: from start) * (gtime_t te) I end time (te.time==0: to end) * (double tint) I time interval (0: all) * (int qflag) I quality flag (0: all) * solbuf_t *solbuf O solution buffer * return : status (1:ok,0:no data or error) *-----------------------------------------------------------------------------*/ // extern int readsolt(char *files[], int nfile, gtime_t ts, gtime_t te, // double tint, int qflag, solbuf_t *solbuf) //{ // FILE *fp; // solopt_t opt=solopt_default; // int i; // // trace(3,"readsolt: nfile=%d\n",nfile); // // initsolbuf(solbuf,0,0); // // for (i=0;icyclic) { /* ring buffer */ // if (solbuf->nmax<=1) return 0; // solbuf->data[solbuf->end]=*sol; // if (++solbuf->end>=solbuf->nmax) solbuf->end=0; // if (solbuf->start==solbuf->end) { // if (++solbuf->start>=solbuf->nmax) solbuf->start=0; // } // else solbuf->n++; // // return 1; // } // if (solbuf->n>=solbuf->nmax) { // solbuf->nmax=solbuf->nmax==0?8192:solbuf->nmax*2; // if (!(solbuf_data=(sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->nmax))) { // trace(1,"addsol: memory allocation error\n"); // free(solbuf->data); solbuf->data=NULL; solbuf->n=solbuf->nmax=0; // return 0; // } // solbuf->data=solbuf_data; // } // solbuf->data[solbuf->n++]=*sol; // return 1; // } /* get solution data from solution buffer -------------------------------------- * get solution data by index from solution buffer * args : solbuf_t *solbuf I solution buffer * int index I index of solution (0...) * return : solution data pointer (NULL: no solution, out of range) *-----------------------------------------------------------------------------*/ // extern sol_t *getsol(solbuf_t *solbuf, int index) //{ // trace(4,"getsol: index=%d\n",index); // // if (index<0||solbuf->n<=index) return NULL; // if ((index=solbuf->start+index)>=solbuf->nmax) { // index-=solbuf->nmax; // } // return solbuf->data+index; // } /* initialize solution buffer -------------------------------------------------- * initialize position solutions * args : solbuf_t *solbuf I solution buffer * int cyclic I solution data buffer type (0:linear,1:cyclic) * int nmax I initial number of solution data * return : status (1:ok,0:error) *-----------------------------------------------------------------------------*/ // extern void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax) //{ //#if 0 // gtime_t time0={0}; //#endif // int i; // // trace(3,"initsolbuf: cyclic=%d nmax=%d\n",cyclic,nmax); // // solbuf->n=solbuf->nmax=solbuf->start=solbuf->end=solbuf->nb=0; // solbuf->cyclic=cyclic; //#if 0 // solbuf->time=time0; //#endif // solbuf->data=NULL; // for (i=0;i<3;i++) { // solbuf->rb[i]=0.0; // } // if (cyclic) { // if (nmax<=2) nmax=2; // if (!(solbuf->data=malloc(sizeof(sol_t)*nmax))) { // trace(1,"initsolbuf: memory allocation error\n"); // return; // } // solbuf->nmax=nmax; // } // } /* free solution --------------------------------------------------------------- * free memory for solution buffer * args : solbuf_t *solbuf I solution buffer * return : none *-----------------------------------------------------------------------------*/ // extern void freesolbuf(solbuf_t *solbuf) //{ // int i; // // trace(3,"freesolbuf: n=%d\n",solbuf->n); // // free(solbuf->data); // solbuf->n=solbuf->nmax=solbuf->start=solbuf->end=solbuf->nb=0; // solbuf->data=NULL; // for (i=0;i<3;i++) { // solbuf->rb[i]=0.0; // } // } extern void freesolstatbuf(solstatbuf_t *solstatbuf) { trace(3, "freesolstatbuf: n=%d\n", solstatbuf->n); solstatbuf->n = solstatbuf->nmax = 0; free(solstatbuf->data); solstatbuf->data = NULL; } /* compare solution status ---------------------------------------------------*/ static int cmpsolstat(const void *p1, const void *p2) { solstat_t *q1 = (solstat_t *)p1, *q2 = (solstat_t *)p2; double tt = timediff(q1->time, q2->time); return tt < -0.0 ? -1 : (tt > 0.0 ? 1 : 0); } /* sort solution data --------------------------------------------------------*/ static int sort_solstat(solstatbuf_t *statbuf) { solstat_t *statbuf_data; trace(4, "sort_solstat: n=%d\n", statbuf->n); if (statbuf->n <= 0) return 0; if (!(statbuf_data = realloc(statbuf->data, sizeof(solstat_t) * statbuf->n))) { trace(1, "sort_solstat: memory allocation error\n"); free(statbuf->data); statbuf->data = NULL; statbuf->n = statbuf->nmax = 0; return 0; } statbuf->data = statbuf_data; qsort(statbuf->data, statbuf->n, sizeof(solstat_t), cmpsolstat); statbuf->nmax = statbuf->n; return 1; } /* decode solution status ----------------------------------------------------*/ static int decode_solstat(char *buff, solstat_t *stat) { static const solstat_t stat0 = {{0}}; double tow, az, el, resp, resc, snr; int n, week, sat, frq, vsat, fix, slip, lock, outc, slipc, rejc; char id[32] = "", *p; trace(4, "decode_solstat: buff=%s\n", buff); if (strstr(buff, "$SAT") != buff) return 0; for (p = buff; *p; p++) if (*p == ',') *p = ' '; n = sscanf(buff, "$SAT%d%lf%s%d%lf%lf%lf%lf%d%lf%d%d%d%d%d%d", &week, &tow, id, &frq, &az, &el, &resp, &resc, &vsat, &snr, &fix, &slip, &lock, &outc, &slipc, &rejc); if (n < 15) { trace(2, "invalid format of solution status: %s\n", buff); return 0; } if ((sat = satid2no(id)) <= 0) { trace(2, "invalid satellite in solution status: %s\n", id); return 0; } *stat = stat0; stat->time = gpst2time(week, tow); stat->sat = (uint8_t)sat; stat->frq = (uint8_t)frq; stat->az = (float)(az * D2R); stat->el = (float)(el * D2R); stat->resp = (float)resp; stat->resc = (float)resc; stat->flag = (uint8_t)((vsat << 5) + (slip << 3) + fix); stat->snr = (uint16_t)(snr / SNR_UNIT + 0.5); stat->lock = (uint16_t)lock; stat->outc = (uint16_t)outc; stat->slipc = (uint16_t)slipc; stat->rejc = (uint16_t)rejc; return 1; } /* add solution status data --------------------------------------------------*/ static void addsolstat(solstatbuf_t *statbuf, const solstat_t *stat) { solstat_t *statbuf_data; trace(4, "addsolstat:\n"); if (statbuf->n >= statbuf->nmax) { statbuf->nmax = statbuf->nmax == 0 ? 8192 : statbuf->nmax * 2; if (!(statbuf_data = (solstat_t *)realloc(statbuf->data, sizeof(solstat_t) * statbuf->nmax))) { trace(1, "addsolstat: memory allocation error\n"); free(statbuf->data); statbuf->data = NULL; statbuf->n = statbuf->nmax = 0; return; } statbuf->data = statbuf_data; } statbuf->data[statbuf->n++] = *stat; } /* read solution status data -------------------------------------------------*/ static int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint, solstatbuf_t *statbuf) { solstat_t stat = {{0}}; char buff[MAXSOLMSG + 1]; trace(3, "readsolstatdata:\n"); while (fgets(buff, sizeof(buff), fp)) { /* decode solution status */ if (!decode_solstat(buff, &stat)) continue; /* add solution to solution buffer */ if (screent(stat.time, ts, te, tint)) { addsolstat(statbuf, &stat); } } return statbuf->n > 0; } /* read solution status -------------------------------------------------------- * read solution status from solution status files * args : char *files[] I solution status files * int nfile I number of files * (gtime_t ts) I start time (ts.time==0: from start) * (gtime_t te) I end time (te.time==0: to end) * (double tint) I time interval (0: all) * solstatbuf_t *statbuf O solution status buffer * return : status (1:ok,0:no data or error) *-----------------------------------------------------------------------------*/ extern int readsolstatt(char *files[], int nfile, gtime_t ts, gtime_t te, double tint, solstatbuf_t *statbuf) { FILE *fp; char path[1024], *p; int i; trace(3, "readsolstatt: nfile=%d\n", nfile); statbuf->n = statbuf->nmax = 0; statbuf->data = NULL; for (i = 0; i < nfile; i++) { if ((p = strrchr(files[i], '.')) && !strcmp(p, ".stat")) { sprintf(path, "%s", files[i]); } else { sprintf(path, "%s.stat", files[i]); } if (!(fp = fopen(path, "r"))) { trace(2, "readsolstatt: file open error %s\n", path); continue; } /* read solution status data */ if (!readsolstatdata(fp, ts, te, tint, statbuf)) { trace(2, "readsolstatt: no solution in %s\n", path); } fclose(fp); } return sort_solstat(statbuf); } extern int readsolstat(char *files[], int nfile, solstatbuf_t *statbuf) { gtime_t time = {0}; trace(3, "readsolstat: nfile=%d\n", nfile); return readsolstatt(files, nfile, time, time, 0.0, statbuf); } /* output solution as the form of x/y/z-ecef ---------------------------------*/ static int outecef(uint8_t *buff, const char *s, const sol_t *sol, const solopt_t *opt) { const char *sep = opt2sep(opt); char *p = (char *)buff; trace(3, "outecef:\n"); p += sprintf(p, "%s%s%14.4f%s%14.4f%s%14.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s" "%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f", s, sep, sol->rr[0], sep, sol->rr[1], sep, sol->rr[2], sep, sol->stat, sep, sol->ns, sep, SQRT(sol->qr[0]), sep, SQRT(sol->qr[1]), sep, SQRT(sol->qr[2]), sep, sqvar(sol->qr[3]), sep, sqvar(sol->qr[4]), sep, sqvar(sol->qr[5]), sep, sol->age, sep, sol->ratio); if (opt->outvel) { /* output velocity */ p += sprintf(p, "%s%10.5f%s%10.5f%s%10.5f%s%9.5f%s%8.5f%s%8.5f%s%8.5f%s" "%8.5f%s%8.5f", sep, sol->rr[3], sep, sol->rr[4], sep, sol->rr[5], sep, SQRT(sol->qv[0]), sep, SQRT(sol->qv[1]), sep, SQRT(sol->qv[2]), sep, sqvar(sol->qv[3]), sep, sqvar(sol->qv[4]), sep, sqvar(sol->qv[5])); } p += sprintf(p, "\r\n"); return (int)(p - (char *)buff); } /* output solution as the form of lat/lon/height -----------------------------*/ static int outpos(uint8_t *buff, const char *s, const sol_t *sol, const solopt_t *opt) { double pos[3], vel[3], dms1[3], dms2[3], P[9], Q[9]; const char *sep = opt2sep(opt); char *p = (char *)buff; trace(3, "outpos :\n"); ecef2pos(sol->rr, pos); soltocov(sol, P); covenu(pos, P, Q); if (opt->height == 1) { /* geodetic height */ pos[2] -= geoidh(pos); } if (opt->degf) { deg2dms(pos[0] * R2D, dms1, 5); deg2dms(pos[1] * R2D, dms2, 5); p += sprintf(p, "%s%s%4.0f%s%02.0f%s%08.5f%s%4.0f%s%02.0f%s%08.5f", s, sep, dms1[0], sep, dms1[1], sep, dms1[2], sep, dms2[0], sep, dms2[1], sep, dms2[2]); } else { p += sprintf(p, "%s%s%14.9f%s%14.9f", s, sep, pos[0] * R2D, sep, pos[1] * R2D); } p += sprintf(p, "%s%10.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f" "%s%6.2f%s%6.1f", sep, pos[2], sep, sol->stat, sep, sol->ns, sep, SQRT(Q[4]), sep, SQRT(Q[0]), sep, SQRT(Q[8]), sep, sqvar(Q[1]), sep, sqvar(Q[2]), sep, sqvar(Q[5]), sep, sol->age, sep, sol->ratio); if (opt->outvel) { /* output velocity */ soltocov_vel(sol, P); ecef2enu(pos, sol->rr + 3, vel); covenu(pos, P, Q); p += sprintf(p, "%s%10.5f%s%10.5f%s%10.5f%s%9.5f%s%8.5f%s%8.5f%s%8.5f%s" "%8.5f%s%8.5f", sep, vel[1], sep, vel[0], sep, vel[2], sep, SQRT(Q[4]), sep, SQRT(Q[0]), sep, SQRT(Q[8]), sep, sqvar(Q[1]), sep, sqvar(Q[2]), sep, sqvar(Q[5])); } p += sprintf(p, "\r\n"); return (int)(p - (char *)buff); } /* output solution as the form of e/n/u-baseline -----------------------------*/ static int outenu(uint8_t *buff, const char *s, const sol_t *sol, const double *rb, const solopt_t *opt) { double pos[3], rr[3], enu[3], P[9], Q[9]; int i; const char *sep = opt2sep(opt); char *p = (char *)buff; trace(3, "outenu :\n"); for (i = 0; i < 3; i++) rr[i] = sol->rr[i] - rb[i]; ecef2pos(rb, pos); soltocov(sol, P); covenu(pos, P, Q); ecef2enu(pos, rr, enu); p += sprintf(p, "%s%s%14.4f%s%14.4f%s%14.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s" "%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\r\n", s, sep, enu[0], sep, enu[1], sep, enu[2], sep, sol->stat, sep, sol->ns, sep, SQRT(Q[0]), sep, SQRT(Q[4]), sep, SQRT(Q[8]), sep, sqvar(Q[1]), sep, sqvar(Q[5]), sep, sqvar(Q[2]), sep, sol->age, sep, sol->ratio); return (int)(p - (char *)buff); } /* output solution in the form of NMEA RMC sentence --------------------------*/ extern int outnmea_rmc(uint8_t *buff, const sol_t *sol) { static double dirp = 0.0; gtime_t time; double ep[6], pos[3], enuv[3], dms1[3], dms2[3], vel, dir, amag = 0.0; char *p = (char *)buff, *q, sum; const char *emag = "E", *mode = "A", *status = "V"; trace(3, "outnmea_rmc:\n"); if (sol->stat <= SOLQ_NONE) { p += sprintf(p, "$%sRMC,,,,,,,,,,,,,", NMEA_TID); for (q = (char *)buff + 1, sum = 0; *q; q++) sum ^= *q; p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); return (int)(p - (char *)buff); } time = gpst2utc(sol->time); if (time.sec >= 0.995) { time.time++; time.sec = 0.0; } time2epoch(time, ep); ecef2pos(sol->rr, pos); ecef2enu(pos, sol->rr + 3, enuv); vel = norm(enuv, 3); if (vel >= 1.0) { dir = atan2(enuv[0], enuv[1]) * R2D; if (dir < 0.0) dir += 360.0; dirp = dir; } else { dir = dirp; } if (sol->stat == SOLQ_DGPS || sol->stat == SOLQ_SBAS) mode = "D"; else if (sol->stat == SOLQ_FLOAT || sol->stat == SOLQ_FIX) mode = "R"; else if (sol->stat == SOLQ_PPP) mode = "P"; deg2dms(fabs(pos[0]) * R2D, dms1, 7); deg2dms(fabs(pos[1]) * R2D, dms2, 7); p += sprintf(p, "$%sRMC,%02.0f%02.0f%05.2f,A,%02.0f%010.7f,%s,%03.0f%010.7f," "%s,%4.2f,%4.2f,%02.0f%02.0f%02d,%.1f,%s,%s,%s", NMEA_TID, ep[3], ep[4], ep[5], dms1[0], dms1[1] + dms1[2] / 60.0, pos[0] >= 0 ? "N" : "S", dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", vel / KNOT2M, dir, ep[2], ep[1], (int)ep[0] % 100, amag, emag, mode, status); for (q = (char *)buff + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ p += sprintf(p, "*%02X\r\n", sum); return (int)(p - (char *)buff); } /* output solution in the form of NMEA GGA sentence --------------------------*/ extern int outnmea_gga(uint8_t *buff, const sol_t *sol) { gtime_t time; double h, ep[6], pos[3], dms1[3], dms2[3], dop = 1.0; int solq, refid = 0; char *p = (char *)buff, *q, sum; trace(3, "outnmea_gga:\n"); if (sol->stat <= SOLQ_NONE) { p += sprintf(p, "$%sGGA,,,,,,,,,,,,,,", NMEA_TID); for (q = (char *)buff + 1, sum = 0; *q; q++) sum ^= *q; p += sprintf(p, "*%02X%c%c", sum, 0x0D, 0x0A); return (int)(p - (char *)buff); } for (solq = 0; solq < 8; solq++) if (nmea_solq[solq] == sol->stat) break; if (solq >= 8) solq = 0; time = gpst2utc(sol->time); if (time.sec >= 0.995) { time.time++; time.sec = 0.0; } time2epoch(time, ep); ecef2pos(sol->rr, pos); h = geoidh(pos); deg2dms(fabs(pos[0]) * R2D, dms1, 7); deg2dms(fabs(pos[1]) * R2D, dms2, 7); p += sprintf(p, "$%sGGA,%02.0f%02.0f%05.2f,%02.0f%010.7f,%s,%03.0f%010.7f,%s," "%d,%02d,%.1f,%.3f,M,%.3f,M,%.1f,%04d", NMEA_TID, ep[3], ep[4], ep[5], dms1[0], dms1[1] + dms1[2] / 60.0, pos[0] >= 0 ? "N" : "S", dms2[0], dms2[1] + dms2[2] / 60.0, pos[1] >= 0 ? "E" : "W", solq, sol->ns, dop, pos[2] - h, h, sol->age, refid); for (q = (char *)buff + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ p += sprintf(p, "*%02X\r\n", sum); return (int)(p - (char *)buff); } /* output solution in the form of NMEA GSA sentences -------------------------*/ extern int outnmea_gsa(uint8_t *buff, const sol_t *sol, const ssat_t *ssat) { double azel[MAXSAT * 2], dop[4]; char *p = (char *)buff, *q, *s, sum; int i, j, sys, prn, nsat, mask = 0, nsys = 0, sats[MAXSAT]; trace(3, "outnmea_gsa:\n"); for (i = nsat = 0; i < MAXSAT; i++) { if (!ssat[i].vs) continue; sys = satsys(i + 1, NULL); if (!(sys & mask)) nsys++; /* # of systems */ mask |= sys; azel[2 * nsat] = ssat[i].azel[0]; azel[2 * nsat + 1] = ssat[i].azel[1]; sats[nsat++] = i + 1; } dops(nsat, azel, 0.0, dop); for (i = 0; nmea_sys[i]; i++) { for (j = nsat = 0; j < MAXSAT && nsat < 12; j++) { if (!(satsys(j + 1, NULL) & nmea_sys[i])) continue; if (ssat[j].vs) sats[nsat++] = j + 1; } if (nsat > 0) { s = p; p += sprintf(p, "$%sGSA,A,%d", nsys > 1 ? "GN" : nmea_tid[i], sol->stat ? 3 : 1); for (j = 0; j < 12; j++) { sys = satsys(sats[j], &prn); if (sys == SYS_SBS) prn -= 87; /* SBS: 33-64 */ else if (sys == SYS_GLO) prn += 64; /* GLO: 65-99 */ else if (sys == SYS_QZS) prn -= 192; /* QZS: 01-10 */ if (j < nsat) p += sprintf(p, ",%02d", prn); else p += sprintf(p, ","); } p += sprintf(p, ",%3.1f,%3.1f,%3.1f,%d", dop[1], dop[2], dop[3], nmea_sid[i]); for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ p += sprintf(p, "*%02X\r\n", sum); } } return (int)(p - (char *)buff); } /* output solution in the form of NMEA GSV sentences -------------------------*/ extern int outnmea_gsv(uint8_t *buff, const sol_t *sol, const ssat_t *ssat) { double az, el, snr; int i, j, k, n, nsat, nmsg, prn, sys, sats[MAXSAT]; char *p = (char *)buff, *q, *s, sum; trace(3, "outnmea_gsv:\n"); for (i = 0; nmea_sys[i]; i++) { for (j = nsat = 0; j < MAXSAT && nsat < 36; j++) { if (!(satsys(j + 1, NULL) & nmea_sys[i])) continue; if (ssat[j].azel[1] > 0.0) sats[nsat++] = j + 1; } nmsg = (nsat + 3) / 4; for (j = n = 0; j < nmsg; j++) { s = p; p += sprintf(p, "$%sGSV,%d,%d,%02d", nmea_tid[i], nmsg, j + 1, nsat); for (k = 0; k < 4; k++, n++) { if (n < nsat) { sys = satsys(sats[n], &prn); if (sys == SYS_SBS) prn -= 87; /* SBS: 33-64 */ else if (sys == SYS_GLO) prn += 64; /* GLO: 65-99 */ else if (sys == SYS_QZS) prn -= 192; /* QZS: 01-10 */ az = ssat[sats[n] - 1].azel[0] * R2D; if (az < 0.0) az += 360.0; el = ssat[sats[n] - 1].azel[1] * R2D; snr = ssat[sats[n] - 1].snr_rover[0] * SNR_UNIT; p += sprintf(p, ",%02d,%02.0f,%03.0f,%02.0f", prn, el, az, snr); } else p += sprintf(p, ",,,,"); } p += sprintf(p, ",0"); /* all signals */ for (q = s + 1, sum = 0; *q; q++) sum ^= *q; /* check-sum */ p += sprintf(p, "*%02X\r\n", sum); } } return (int)(p - (char *)buff); } /* output processing options --------------------------------------------------- * output processing options to buffer * args : uint8_t *buff IO output buffer * prcopt_t *opt I processing options * return : number of output bytes *-----------------------------------------------------------------------------*/ extern int outprcopts(uint8_t *buff, const prcopt_t *opt) { const int sys[] = { SYS_GPS, SYS_GLO, SYS_GAL, SYS_QZS, SYS_CMP, SYS_IRN, SYS_SBS, 0}; const char *s1[] = { "Single", "DGPS", "Kinematic", "Static", "Static-Start", "Moving-Base", "Fixed", "PPP Kinematic", "PPP Static", "PPP Fixed", "", "", ""}; const char *s2[] = { "L1", "L1+L2/E5b", "L1+L2/E5b+L5", "L1+L2/E5b+L5+L6", "L1+2+3+4+5", "L1+2+3+4+5+6", "", "", ""}; const char *s3[] = { "Forward", "Backward", "Combined-Phase Reset", "Combined-No Phase Reset", "", ""}; const char *s4[] = { "OFF", "Broadcast", "SBAS", "Iono-Free LC", "Estimate TEC", "IONEX TEC", "QZSS Broadcast", "", "", "", ""}; const char *s5[] = { "OFF", "Saastamoinen", "SBAS", "Estimate ZTD", "Estimate ZTD+Grad", "", "", ""}; const char *s6[] = { "Broadcast", "Precise", "Broadcast+SBAS", "Broadcast+SSR APC", "Broadcast+SSR CoM", "", "", ""}; const char *s7[] = { "GPS", "GLONASS", "Galileo", "QZSS", "BDS", "NavIC", "SBAS", "", "", ""}; const char *s8[] = { "OFF", "Continuous", "Instantaneous", "Fix and Hold", "", "", ""}; const char *s9[] = { "OFF", "ON", "AutoCal", "Fix and Hold", ""}; int i; char *p = (char *)buff; trace(3, "outprcopts:\n"); p += sprintf(p, "%s pos mode : %s\r\n", COMMENTH, s1[opt->mode]); if (PMODE_DGPS <= opt->mode && opt->mode <= PMODE_FIXED) { p += sprintf(p, "%s freqs : %s\r\n", COMMENTH, s2[opt->nf - 1]); } if (opt->mode > PMODE_SINGLE) { p += sprintf(p, "%s solution : %s\r\n", COMMENTH, s3[opt->soltype]); } p += sprintf(p, "%s elev mask : %.1f deg\r\n", COMMENTH, opt->elmin * R2D); if (opt->mode > PMODE_SINGLE) { p += sprintf(p, "%s dynamics : %s\r\n", COMMENTH, opt->dynamics ? "on" : "off"); p += sprintf(p, "%s tidecorr : %s\r\n", COMMENTH, opt->tidecorr ? "on" : "off"); } if (opt->mode <= PMODE_FIXED) { p += sprintf(p, "%s ionos opt : %s\r\n", COMMENTH, s4[opt->ionoopt]); } p += sprintf(p, "%s tropo opt : %s\r\n", COMMENTH, s5[opt->tropopt]); p += sprintf(p, "%s ephemeris : %s\r\n", COMMENTH, s6[opt->sateph]); p += sprintf(p, "%s navi sys :", COMMENTH); for (i = 0; sys[i]; i++) { if (opt->navsys & sys[i]) p += sprintf(p, " %s", s7[i]); } p += sprintf(p, "\r\n"); if (PMODE_KINEMA <= opt->mode && opt->mode <= PMODE_FIXED) { p += sprintf(p, "%s amb res : %s\r\n", COMMENTH, s8[opt->modear]); if (opt->navsys & SYS_GLO) { p += sprintf(p, "%s amb glo : %s\r\n", COMMENTH, s9[opt->glomodear]); } if (opt->thresar[0] > 0.0) { p += sprintf(p, "%s val thres : %.1f\r\n", COMMENTH, opt->thresar[0]); } } if (opt->mode == PMODE_MOVEB && opt->baseline[0] > 0.0) { p += sprintf(p, "%s baseline : %.4f %.4f m\r\n", COMMENTH, opt->baseline[0], opt->baseline[1]); } for (i = 0; i < 2; i++) { if (opt->mode == PMODE_SINGLE || (i >= 1 && opt->mode > PMODE_FIXED)) continue; p += sprintf(p, "%s antenna%d : %-21s (%7.4f %7.4f %7.4f)\r\n", COMMENTH, i + 1, opt->anttype[i], opt->antdel[i][0], opt->antdel[i][1], opt->antdel[i][2]); } return (int)(p - (char *)buff); } /* output solution header ------------------------------------------------------ * output solution header to buffer * args : uint8_t *buff IO output buffer * solopt_t *opt I solution options * return : number of output bytes *-----------------------------------------------------------------------------*/ extern int outsolheads(uint8_t *buff, const solopt_t *opt) { const char *s1[] = {"WGS84", "Tokyo"}, *s2[] = {"ellipsoidal", "geodetic"}; const char *s3[] = {"GPST", "UTC ", "JST "}, *sep = opt2sep(opt); const char *leg1 = "Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp"; const char *leg2 = "ns=# of satellites"; char *p = (char *)buff; int timeu = opt->timeu < 0 ? 0 : (opt->timeu > 20 ? 20 : opt->timeu); trace(3, "outsolheads:\n"); if (opt->posf == SOLF_NMEA || opt->posf == SOLF_STAT || opt->posf == SOLF_GSIF) { return 0; } if (opt->outhead) { p += sprintf(p, "%s (", COMMENTH); if (opt->posf == SOLF_XYZ) p += sprintf(p, "x/y/z-ecef=WGS84"); else if (opt->posf == SOLF_ENU) p += sprintf(p, "e/n/u-baseline=WGS84"); else p += sprintf(p, "lat/lon/height=%s/%s", s1[opt->datum], s2[opt->height]); p += sprintf(p, ",%s,%s)\r\n", leg1, leg2); } p += sprintf(p, "%s %-*s%s", COMMENTH, (opt->timef ? 16 : 8) + timeu + 1, s3[opt->times], sep); if (opt->posf == SOLF_LLH) { /* lat/lon/hgt */ if (opt->degf) { p += sprintf(p, "%16s%s%16s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s" "%8s%s%6s%s%6s", "latitude(d'\")", sep, "longitude(d'\")", sep, "height(m)", sep, "Q", sep, "ns", sep, "sdn(m)", sep, "sde(m)", sep, "sdu(m)", sep, "sdne(m)", sep, "sdeu(m)", sep, "sdue(m)", sep, "age(s)", sep, "ratio"); } else { p += sprintf(p, "%14s%s%14s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s" "%8s%s%6s%s%6s", "latitude(deg)", sep, "longitude(deg)", sep, "height(m)", sep, "Q", sep, "ns", sep, "sdn(m)", sep, "sde(m)", sep, "sdu(m)", sep, "sdne(m)", sep, "sdeu(m)", sep, "sdun(m)", sep, "age(s)", sep, "ratio"); } if (opt->outvel) { p += sprintf(p, "%s%10s%s%10s%s%10s%s%9s%s%8s%s%8s%s%8s%s%8s%s%8s", sep, "vn(m/s)", sep, "ve(m/s)", sep, "vu(m/s)", sep, "sdvn", sep, "sdve", sep, "sdvu", sep, "sdvne", sep, "sdveu", sep, "sdvun"); } } else if (opt->posf == SOLF_XYZ) { /* x/y/z-ecef */ p += sprintf(p, "%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s" "%s%6s%s%6s", "x-ecef(m)", sep, "y-ecef(m)", sep, "z-ecef(m)", sep, "Q", sep, "ns", sep, "sdx(m)", sep, "sdy(m)", sep, "sdz(m)", sep, "sdxy(m)", sep, "sdyz(m)", sep, "sdzx(m)", sep, "age(s)", sep, "ratio"); if (opt->outvel) { p += sprintf(p, "%s%10s%s%10s%s%10s%s%9s%s%8s%s%8s%s%8s%s%8s%s%8s", sep, "vx(m/s)", sep, "vy(m/s)", sep, "vz(m/s)", sep, "sdvx", sep, "sdvy", sep, "sdvz", sep, "sdvxy", sep, "sdvyz", sep, "sdvzx"); } } else if (opt->posf == SOLF_ENU) { /* e/n/u-baseline */ p += sprintf(p, "%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s" "%s%6s%s%6s", "e-baseline(m)", sep, "n-baseline(m)", sep, "u-baseline(m)", sep, "Q", sep, "ns", sep, "sde(m)", sep, "sdn(m)", sep, "sdu(m)", sep, "sden(m)", sep, "sdnu(m)", sep, "sdue(m)", sep, "age(s)", sep, "ratio"); if (opt->outvel) { p += sprintf(p, "%s%10s%s%10s%s%10s%s%9s%s%8s%s%8s%s%8s%s%8s%s%8s", sep, "ve(m/s)", sep, "vn(m/s)", sep, "vu(m/s)", sep, "sdve", sep, "sdvn", sep, "sdvu", sep, "sdven", sep, "sdvnu", sep, "sdvue"); } } p += sprintf(p, "\r\n"); return (int)(p - (char *)buff); } /* std-dev of soltuion -------------------------------------------------------*/ static double sol_std(const sol_t *sol) { /* approximate as max std-dev of 3-axis std-devs */ if (sol->qr[0] > sol->qr[1] && sol->qr[0] > sol->qr[2]) return SQRT(sol->qr[0]); if (sol->qr[1] > sol->qr[2]) return SQRT(sol->qr[1]); return SQRT(sol->qr[2]); } /* output solution body -------------------------------------------------------- * output solution body to buffer * args : uint8_t *buff IO output buffer * sol_t *sol I solution * double *rb I base station position {x,y,z} (ecef) (m) * solopt_t *opt I solution options * return : number of output bytes *-----------------------------------------------------------------------------*/ extern int outsols(uint8_t *buff, const sol_t *sol, const double *rb, const solopt_t *opt) { gtime_t time, ts = {0}; double gpst; int week, timeu; const char *sep = opt2sep(opt); char s[64]; uint8_t *p = buff; trace(3, "outsols :\n"); /* suppress output if std is over opt->maxsolstd */ if (opt->maxsolstd > 0.0 && sol_std(sol) > opt->maxsolstd) { return 0; } if (opt->posf == SOLF_NMEA) { if (opt->nmeaintv[0] < 0.0) return 0; if (!screent(sol->time, ts, ts, opt->nmeaintv[0])) return 0; } if (sol->stat <= SOLQ_NONE || (opt->posf == SOLF_ENU && norm(rb, 3) <= 0.0)) { return 0; } timeu = opt->timeu < 0 ? 0 : (opt->timeu > 20 ? 20 : opt->timeu); time = sol->time; if (opt->times >= TIMES_UTC) time = gpst2utc(time); if (opt->times == TIMES_JST) time = timeadd(time, 9 * 3600.0); if (opt->timef) time2str(time, s, timeu); else { gpst = time2gpst(time, &week); if (86400 * 7 - gpst < 0.5 / pow(10.0, timeu)) { week++; gpst = 0.0; } sprintf(s, "%4d%.16s%*.*f", week, sep, 6 + (timeu <= 0 ? 0 : timeu + 1), timeu, gpst); } switch (opt->posf) { case SOLF_LLH: p += outpos(p, s, sol, opt); break; case SOLF_XYZ: p += outecef(p, s, sol, opt); break; case SOLF_ENU: p += outenu(p, s, sol, rb, opt); break; case SOLF_NMEA: p += outnmea_rmc(p, sol); p += outnmea_gga(p, sol); break; } return (int)(p - buff); } /* output solution extended ---------------------------------------------------- * output solution exteneded infomation * args : uint8_t *buff IO output buffer * sol_t *sol I solution * ssat_t *ssat I satellite status * solopt_t *opt I solution options * return : number of output bytes * notes : only support nmea *-----------------------------------------------------------------------------*/ extern int outsolexs(uint8_t *buff, const sol_t *sol, const ssat_t *ssat, const solopt_t *opt) { gtime_t ts = {0}; uint8_t *p = buff; trace(3, "outsolexs:\n"); /* suppress output if std is over opt->maxsolstd */ if (opt->maxsolstd > 0.0 && sol_std(sol) > opt->maxsolstd) { return 0; } if (opt->posf == SOLF_NMEA) { if (opt->nmeaintv[1] < 0.0) return 0; if (!screent(sol->time, ts, ts, opt->nmeaintv[1])) return 0; } if (opt->posf == SOLF_NMEA) { p += outnmea_gsa(p, sol, ssat); p += outnmea_gsv(p, sol, ssat); } return (int)(p - buff); } /* output processing option ---------------------------------------------------- * output processing option to file * args : FILE *fp I output file pointer * prcopt_t *opt I processing options * return : none *-----------------------------------------------------------------------------*/ extern void outprcopt(FILE *fp, const prcopt_t *opt) { uint8_t buff[MAXSOLMSG + 1]; int n; trace(3, "outprcopt:\n"); if ((n = outprcopts(buff, opt)) > 0) { fwrite(buff, n, 1, fp); } } /* output solution header ------------------------------------------------------ * output solution heade to file * args : FILE *fp I output file pointer * solopt_t *opt I solution options * return : none *-----------------------------------------------------------------------------*/ extern void outsolhead(FILE *fp, const solopt_t *opt) { uint8_t buff[MAXSOLMSG + 1]; int n; trace(3, "outsolhead:\n"); if ((n = outsolheads(buff, opt)) > 0) { fwrite(buff, n, 1, fp); } } /* output solution body -------------------------------------------------------- * output solution body to file * args : FILE *fp I output file pointer * sol_t *sol I solution * double *rb I base station position {x,y,z} (ecef) (m) * solopt_t *opt I solution options * return : none *-----------------------------------------------------------------------------*/ extern void outsol(FILE *fp, const sol_t *sol, const double *rb, const solopt_t *opt) { uint8_t buff[MAXSOLMSG + 1]; int n; trace(3, "outsol :\n"); if ((n = outsols(buff, sol, rb, opt)) > 0) { fwrite(buff, n, 1, fp); } } /* output solution extended ---------------------------------------------------- * output solution exteneded infomation to file * args : FILE *fp I output file pointer * sol_t *sol I solution * ssat_t *ssat I satellite status * solopt_t *opt I solution options * return : output size (bytes) * notes : only support nmea *-----------------------------------------------------------------------------*/ extern void outsolex(FILE *fp, const sol_t *sol, const ssat_t *ssat, const solopt_t *opt) { uint8_t buff[MAXSOLMSG + 1]; int n; trace(3, "outsolex:\n"); if ((n = outsolexs(buff, sol, ssat, opt)) > 0) { fwrite(buff, n, 1, fp); } }