From 63827d819518ad3e8e794741db65dd13884fc20c Mon Sep 17 00:00:00 2001 From: bert hubert Date: Tue, 25 Feb 2020 12:12:35 +0100 Subject: [PATCH] move to new unified processing model, plus hook up sbas --- Makefile | 6 +- beidou.hh | 8 +- ephemeris.hh | 31 ++ galileo.hh | 18 +- galmonmon.cc | 40 +- glonass.cc | 23 + glonass.hh | 24 +- gps.cc | 119 +++++ gps.hh | 191 +------- html/doalles.js | 11 +- html/observer.js | 4 +- navdump.cc | 192 ++++++-- navmon.cc | 4 +- navparse.cc | 1188 +++++++++++++++++++++++++--------------------- navparse.hh | 136 ++---- sbas.hh | 7 +- 16 files changed, 1120 insertions(+), 882 deletions(-) diff --git a/Makefile b/Makefile index 0c7d179..2cc849e 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ CFLAGS = -O3 -Wall -ggdb -CXXFLAGS:= -std=gnu++17 -Wall -O2 -MMD -MP -fno-omit-frame-pointer -Iext/CLI11 \ +CXXFLAGS:= -std=gnu++17 -Wall -O2 -ggdb -MMD -MP -fno-omit-frame-pointer -Iext/CLI11 \ -Iext/fmt-6.1.2/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ \ -I/usr/local/opt/openssl/include/ \ -Iext/sgp4/libsgp4/ \ @@ -36,7 +36,7 @@ clean: decrypt: decrypt.o bits.o ext/fmt-6.1.2/src/format.o $(CXX) -std=gnu++17 $^ -o $@ -navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o +navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o sbas.o $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY) reporter: reporter.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o githash.o @@ -46,7 +46,7 @@ galmonmon: galmonmon.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ub $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lprotobuf -lcurl -navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o ${EXTRADEP} +navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o sbas.o ${EXTRADEP} $(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -pthread -lprotobuf -lz navdisplay: navdisplay.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o osen.o githash.o diff --git a/beidou.hh b/beidou.hh index 021d955..0276a6f 100644 --- a/beidou.hh +++ b/beidou.hh @@ -17,7 +17,7 @@ int beidouBitconv(int their); C05 (58.75E) */ -struct BeidouMessage +struct BeidouMessage : GPSLikeEphemeris { uint8_t strtype; @@ -118,7 +118,11 @@ struct BeidouMessage double getCrc() const { return ldexp(crc, -6); } // meters double getCrs() const { return ldexp(crs, -6); } // meters double getM0() const { return ldexp(m0 * M_PI, -31); } // radians - + + int getIOD() const + { + return -1; + } void parse2(std::basic_string_view cond) { diff --git a/ephemeris.hh b/ephemeris.hh index bdbb487..5b6eaae 100644 --- a/ephemeris.hh +++ b/ephemeris.hh @@ -2,6 +2,37 @@ #include "minivec.hh" #include #include +#include +struct GPSLikeEphemeris +{ + virtual double getMu() const = 0; + virtual double getOmegaE() const = 0; + virtual double getE() const = 0; + virtual uint32_t getT0e() const = 0; + + virtual double getI0() const = 0; + + virtual double getOmegadot() const = 0; + + virtual double getSqrtA() const = 0; + virtual double getOmega0() const = 0; + virtual double getOmega() const = 0; + + virtual double getM0() const = 0; + virtual double getIdot() const = 0; + virtual double getCic() const = 0; + virtual double getCis() const = 0; + virtual double getCuc() const = 0; + virtual double getCus() const = 0; + virtual double getCrc() const = 0; + virtual double getCrs() const = 0; + virtual double getDeltan()const = 0; + + virtual int getIOD() const = 0; + // maybe af0, af1, af2? + // maybe getUTCOffset? getAtomicOffset etc + +}; // lat, lon, height (rad, rad, meters) std::tuple ecefToWGS84(double x, double y, double z); diff --git a/galileo.hh b/galileo.hh index 4d2b659..7f08cde 100644 --- a/galileo.hh +++ b/galileo.hh @@ -8,7 +8,7 @@ bool getTOWFromInav(std::basic_string_view inav, uint32_t *satTOW, uint16_t *wn); -struct GalileoMessage +struct GalileoMessage : GPSLikeEphemeris { uint8_t wtype; @@ -41,8 +41,8 @@ struct GalileoMessage } uint8_t sparetime{0}; - uint16_t wn{0}; // we put the "unrolled" week number here! - uint32_t tow{0}; // "last seen" + uint16_t wn{0}; + uint32_t tow{0}; int iodalmanac; int wnalmanac; int t0almanac; @@ -99,6 +99,11 @@ struct GalileoMessage uint16_t iodnav; + int getIOD() const + { + return iodnav; + } + struct Almanac { int svid{-1}; @@ -131,7 +136,10 @@ struct GalileoMessage double getCrc() const { return 0; } // meters double getCrs() const { return 0; } // meters double getDeltan()const { return 0; } //radians/s - + int getIOD() const + { + return -1; + } } alma1, alma2, alma3; @@ -211,7 +219,7 @@ struct GalileoMessage return {factor * cur, factor * trend}; } - + // pair of nanosecond, nanosecond/s std::pair getGPSOffset(int tow, int wn) const { int dw = (int)(uint8_t)wn - (int)(uint8_t) wn0g; diff --git a/galmonmon.cc b/galmonmon.cc index 443d81e..07b20b2 100644 --- a/galmonmon.cc +++ b/galmonmon.cc @@ -166,7 +166,7 @@ std::optional StateKeeper::reportState(string_view thing, string_view na StateKeeper g_sk; - +#if 0 static std::string string_replace(const std::string& str, const std::string& match, const std::string& replacement, unsigned int max_replacements = UINT_MAX) { @@ -182,12 +182,14 @@ static std::string string_replace(const std::string& str, const std::string& mat } return newstr; } - +#endif void sendTweet(const string& tweet) { string etweet = tweet; //system((string("twurl -X POST /1.1/statuses/update.json -d \"media_ids=1215649475231997953&status=")+etweet+"\" >> twitter.log").c_str()); - system((string("twurl -X POST /1.1/statuses/update.json -d \"status=")+etweet+"\" >> twitter.log").c_str()); + if(system((string("twurl -X POST /1.1/statuses/update.json -d \"status=")+etweet+"\" >> twitter.log").c_str()) < 0) { + cout<<"Problem tweeting!"< sbasHealthChange; + for(auto iter = j.begin(); iter != j.end(); ++iter) { + if(iter.value().count("health")) { + string name = sbasName(atoi(iter.key().c_str())); + sbasHealthChange = g_sk.reportState(name, "sbas-health", (string)iter.value()["health"]); + // cout<<"Setting state for "<< name <<" to "<< (string)iter.value()["health"] << endl; + + + if(sbasHealthChange) { + ostringstream out; + out<<"✈️ augmentation system "<text<<", old: " << prevState->text; + if(get(state->state) > 3 || get(prevState->state) > 3) out << tmp.str(); + else + cout<<"Not reporting: "< #include +#include +#include +using std::cout; +using std::endl; static const double ae = 6378.136; // km // IERS: 6378136.6 static const double mu = 398.6004418E3; // km3/s2 // IERS: 3.986004418 @@ -98,8 +102,25 @@ static void rk4step (const double A[3], double y[6], double h) y[j] = y[j] + h * (k1[j] + 2*(k2[j] + k3[j]) + k4[j]) / 6; } + +using Clock = std::chrono::steady_clock; + +static double passedMsec(const Clock::time_point& then, const Clock::time_point& now) +{ + return std::chrono::duration_cast(now - then).count()/1000.0; +} + + +static double passedMsec(const Clock::time_point& then) +{ + return passedMsec(then, Clock::now()); +} + + double getCoordinates(double tow, const GlonassMessage& eph, Point* p) { + auto start = Clock::now(); + double y0[6] = {ldexp(eph.x, -11), ldexp(eph.y, -11), ldexp(eph.z, -11), ldexp(eph.dx, -20), ldexp(eph.dy, -20), ldexp(eph.dz, -20)}; double A[3] = {ldexp(eph.ddx, -30), ldexp(eph.ddy, -30), ldexp(eph.ddz, -30)}; @@ -116,5 +137,7 @@ double getCoordinates(double tow, const GlonassMessage& eph, Point* p) rk4step (A, y0, h); *p = Point (1E3*y0[0], 1E3*y0[1], 1E3*y0 [2]); + static double total=0; + // cout<<"Took: "<<(total+=passedMsec(start))<<" ms" < getUTCOffset(int tow) const + { + std::pair ret; + ret.second=0; + ret.first = 1000000000.0*ldexp(tauc, -31); // this is Glonass-M + + return ret; + } + + std::pair getGPSOffset(int tow) const + { + std::pair ret; + ret.second=0; + ret.first = 1000000000.0*ldexp(taugps, -30); + return ret; + } + + uint32_t getGloTime() const; uint8_t n4{0}; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in @@ -138,7 +160,7 @@ struct GlonassMessage { n4=getbitu(&gstr[0], 85-36, 5); taugps = getbitsglonass(&gstr[0], 85-31, 22); - tauc = getbitsglonass(&gstr[0], 85-69, 32); + tauc = getbitsglonass(&gstr[0], 85-69, 32); // check the NEW ICD l_n = getbitu(&gstr[0], 85 - 9, 1); } diff --git a/gps.cc b/gps.cc index e02849b..0a02094 100644 --- a/gps.cc +++ b/gps.cc @@ -13,3 +13,122 @@ std::basic_string getCondensedGPSMessage(std::basic_string_view cond, uint8_t* pageptr) +{ + using namespace std; + int frame = getbitu(&cond[0], 24+19, 3); + // 10 * 4 bytes in payload now + tow = 1.5*(getbitu(&cond[0], 24, 17)*4); + // cerr << "Preamble: "< 56 + // page 25 -> 63 + // 2-10 -> 25 -> 32 ?? + } + + if(frame == 5 || frame==4) { // this is a caroussel frame + gpsalma.dataid = getbitu(&cond[0], 2*24, 2); + gpsalma.sv = getbitu(&cond[0], 2*24+2, 6); + + if(pageptr) + *pageptr= gpsalma.sv; + + + gpsalma.e = getbitu(&cond[0], 2*24 + 8, 16); + gpsalma.t0a = getbitu(&cond[0], 3*24, 8); + gpsalma.deltai = getbits(&cond[0], 3*24 +8 , 16); + gpsalma.omegadot = getbits(&cond[0], 4*24, 16); + gpsalma.health = getbitu(&cond[0], 4*24 +16, 8); + gpsalma.sqrtA = getbitu(&cond[0], 5*24, 24); + gpsalma.Omega0 = getbits(&cond[0], 6*24, 24); + gpsalma.omega = getbits(&cond[0], 7*24, 24); + gpsalma.M0 = getbits(&cond[0], 8*24, 24); + gpsalma.af0 = (getbits(&cond[0], 9*24, 8) << 3) + getbits(&cond[0], 9*24 +19, 3); + gpsalma.af1 = getbits(&cond[0], 9*24 + 8, 11); + // cerr<<"Frame 5, SV: "< #include +#include "ephemeris.hh" + std::basic_string getCondensedGPSMessage(std::basic_string_view payload); -struct GPSAlmanac + +struct GPSAlmanac : GPSLikeEphemeris { int dataid{-1}; int sv; @@ -29,7 +32,7 @@ struct GPSAlmanac { return ldexp(e, -21); } - double getT0e() const + uint32_t getT0e() const { return ldexp(t0a, 12); } @@ -71,30 +74,23 @@ struct GPSAlmanac double getCrs() const { return 0; } // meters double getDeltan()const { return 0; } //radians/s - + int getIOD() const { return 0; } // XXX ioda? }; -struct GPSState +struct GPSState : GPSLikeEphemeris { - struct SVIOD - { - std::bitset<32> words; - int gnssid; - uint32_t t0e; - uint32_t e, sqrtA; - int32_t m0, omega0, i0, omega, idot, omegadot, deltan; - + uint32_t t0e; + uint32_t e, sqrtA; + int32_t m0, omega0, i0, omega, idot, omegadot, deltan; + int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0}; + // 16 seconds + uint16_t t0c; - int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0}; - // 16 seconds - uint16_t t0c; - - // 2^-31 2^-43 - int32_t af0 , af1; - // ??? - int8_t af2; - uint32_t wn{0}, tow{0}; + // 2^-31 2^-43 + int32_t af0 , af1; + // ??? + int8_t af2; double getMu() const { @@ -120,17 +116,12 @@ struct GPSState double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s double getOmega() const { return ldexp(omega * M_PI, -31); } // radians - - }; - GPSAlmanac gpsalma; uint8_t gpshealth{0}; uint16_t ai0{0}; int16_t ai1{0}, ai2{0}; - int BGDE1E5a{0}, BGDE1E5b{0}; - bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false}; uint16_t wn{0}; // we put the "unrolled" week number here! uint32_t tow{0}; // "last seen" @@ -142,27 +133,15 @@ struct GPSState uint16_t wnLSF{0}; uint8_t dn; // leap second day number // 1 2^-31 2^-43 2^-55 16 second - int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX + int ura; int gpsiod{-1}; - - std::map iods; - SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor - void checkCompleteAndClean(int iod) + + int getIOD() const { - if(iods[iod].words[2] && iods[iod].words[3]) { - if(iods.size() > 1) { - auto tmp = iods[iod]; - iods.clear(); - iods[iod] = tmp; - } - } + return gpsiod; } - bool isComplete(int iod) - { - return iods[iod].words[2] && iods[iod].words[3]; - } - + int parseGPSMessage(std::basic_string_view cond, uint8_t* pageptr=0); }; template @@ -203,129 +182,3 @@ std::pair getGPSUTCOffset(int tow, int wn, const T& eph) -// expects input as 24 bit read to to use messages, returns frame number -template -int parseGPSMessage(std::basic_string_view cond, T& out, uint8_t* pageptr=0) -{ - using namespace std; - int frame = getbitu(&cond[0], 24+19, 3); - // 10 * 4 bytes in payload now - out.tow = 1.5*(getbitu(&cond[0], 24, 17)*4); - // cerr << "Preamble: "< 56 - // page 25 -> 63 - // 2-10 -> 25 -> 32 ?? - } - - if(frame == 5 || frame==4) { // this is a caroussel frame - out.gpsalma.dataid = getbitu(&cond[0], 2*24, 2); - out.gpsalma.sv = getbitu(&cond[0], 2*24+2, 6); - - if(pageptr) - *pageptr= out.gpsalma.sv; - - - out.gpsalma.e = getbitu(&cond[0], 2*24 + 8, 16); - out.gpsalma.t0a = getbitu(&cond[0], 3*24, 8); - out.gpsalma.deltai = getbits(&cond[0], 3*24 +8 , 16); - out.gpsalma.omegadot = getbits(&cond[0], 4*24, 16); - out.gpsalma.health = getbitu(&cond[0], 4*24 +16, 8); - out.gpsalma.sqrtA = getbitu(&cond[0], 5*24, 24); - out.gpsalma.Omega0 = getbits(&cond[0], 6*24, 24); - out.gpsalma.omega = getbits(&cond[0], 7*24, 24); - out.gpsalma.M0 = getbits(&cond[0], 8*24, 24); - out.gpsalma.af0 = (getbits(&cond[0], 9*24, 8) << 3) + getbits(&cond[0], 9*24 +19, 3); - out.gpsalma.af1 = getbits(&cond[0], 9*24 + 8, 11); - // cerr<<"Frame 5, SV: "<"+d["utc-offset-ns"].toFixed(2)+" ns, Galileo-GPS offset: "+d["gps-offset-ns"].toFixed(2)+" ns, GPS UTC offset: "+d["gps-utc-offset-ns"].toFixed(2)+". "+d["leap-seconds"]+" leap seconds"); + console.log(d); + var str="Galileo-UTC offset: "+d["gst-utc-offset-ns"].toFixed(2)+" ns"; + str += ", Galileo-GPS offset: "+d["gst-gps-offset-ns"].toFixed(2)+" ns"; + str += " , GPS-UTC offset: "+d["gps-utc-offset-ns"].toFixed(2)+" ns"; + str += " , BeiDou-UTC offset: "+d["beidou-utc-offset-ns"].toFixed(2)+" ns"; + str += " , GLONASS-UTC offset: "+d["glonass-utc-offset-ns"].toFixed(2)+" ns"; + str += " , GLONASS-GPS offset: "+d["glonass-gps-offset-ns"].toFixed(2)+" ns"; + str += ", GPS-UTC offset: "+d["beidou-utc-offset-ns"].toFixed(2)+" ns, "+d["leap-seconds"]+" leap seconds"; + + d3.select('#facts').html(str); lastseen = moment(1000*d["last-seen"]); d3.select("#freshness").html(lastseen.fromNow()); }); diff --git a/html/observer.js b/html/observer.js index a594db6..18516a0 100644 --- a/html/observer.js +++ b/html/observer.js @@ -137,6 +137,7 @@ function makeTable(str, obj) Object.keys(obj).forEach(function(e) { if(e=="svs") { Object.keys(obj[e]).forEach(function(k) { + if(obj[e][k].elev && obj[e][k].azi) { var obj2 ={id: k, elev: obj[e][k].elev.toFixed(1), sigid: obj[e][k].sigid, db: obj[e][k].db, azi: obj[e][k].azi.toFixed(1), @@ -162,6 +163,7 @@ function makeTable(str, obj) else if(gnssid==6) color="yellow"; gnss_position.push([obj[e][k].azi, obj[e][k].elev, k.split("@")[0] , obj[e][k].db/4,4, color]); + } }); } else @@ -215,7 +217,7 @@ function makeTable(str, obj) return ret; })}). - enter().append("td").text(function(d) { + enter().append("td").html(function(d) { return d.value; }).attr("align", d=> d.align).style("background-color", d=> d.color); diff --git a/navdump.cc b/navdump.cc index 79ae6b1..e1c44c0 100644 --- a/navdump.cc +++ b/navdump.cc @@ -26,7 +26,7 @@ #include "sp3.hh" #include "ubx.hh" #include - +#include "sbas.hh" #include "version.hh" #include "gpscnav.hh" #include "rinex.hh" @@ -37,7 +37,7 @@ using namespace std; extern const char* g_gitHash; -Point g_ourpos; +map g_srcpos; vector g_sp3s; @@ -126,19 +126,27 @@ struct SVFilter pos = str.find(',', pos+1); if(pos != string::npos) satid.sigid = atoi(&str[pos+1]); + // cout<<"Add to filter "< d_filter; @@ -179,7 +187,7 @@ void emitFixState(int src, double iTow, FixStat& fs, int n) Point sat; getCoordinates(iTow, s.second.ephemeris, &sat); - if(getElevationDeg(sat, g_ourpos) < 20) + if(getElevationDeg(sat, g_srcpos[src]) < 20) continue; /* Point sat; @@ -187,10 +195,10 @@ void emitFixState(int src, double iTow, FixStat& fs, int n) (void)trend; getCoordinates(iTow + toffset/1000000000.0 + dt, s.second.ephemeris, &sat); - double range = Vector(g_ourpos, sat).length(); + double range = Vector(g_srcpos[nmm.sourceid()], sat).length(); getCoordinates(iTow + range/299792458.0 + toffset/1000000000.0 + dt, s.second.ephemeris, &sat); - range = Vector(g_ourpos, sat).length(); + range = Vector(g_srcpos[nmm.sourceid()], sat).length(); */ double range = s.second.ephrange; if(s.second.bestrange1 != -1) { @@ -214,13 +222,14 @@ void emitFixState(int src, double iTow, FixStat& fs, int n) for(const auto& s : fs.sats) { Point sat; double E=getCoordinates(iTow, s.second.ephemeris, &sat); - cout<<""<t) <<" to "<< humanTime(g_sp3s.rbegin()->t)< svpairs; vector stations; bool doReceptionData{false}; @@ -277,6 +286,7 @@ try ofstream iodstream("iodstream.csv"); iodstream << "timestamp gnssid sv iodnav t0e age" << endl; + ofstream csv("delta.csv"); csv <<"timestamp gnssid sv tow tle_distance alma_distance utc_dist x y z vx vy vz rad inclination e iod"<((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size())); struct GPSState gs; static map eph; + static map almas; uint8_t page; static int gpswn; - int frame=parseGPSMessage(cond, gs, &page); + int frame=gs.parseGPSMessage(cond, &page); cout<<"GPS "< oldgs1s; + static map oldgs2s; if(frame == 1) { - static map oldgs1s; + gpswn = gs.wn; cout << "gpshealth = "<<(int)gs.gpshealth<<", wn "<second.t0c != gs.t0c) { @@ -543,21 +551,23 @@ try oldgs1s[sv] = gs; } else if(frame == 2) { - parseGPSMessage(cond, eph[sv], &page); - cout << "t0e = "<second.t0e << " " <second.t0e) << " iod "<gnss == e.gnss && bestSP3->sv == sv) { + static set> haveSeen; + if(!haveSeen.count({sv, bestSP3->t})) { + haveSeen.insert({sv, bestSP3->t}); + Point newPoint; + double E=getCoordinates(gs.tow + (bestSP3->t - start), eph[sv], &newPoint, false); + Point sp3Point(bestSP3->x, bestSP3->y, bestSP3->z); + Vector dist(newPoint, sp3Point); + + Vector nspeed; + getSpeed(gs.tow + (bestSP3->t - start), eph[sv], &nspeed); + Vector speed = nspeed; + nspeed.norm(); + double along = nspeed.inner(dist); + + cout<<"\nsp3 "<<(bestSP3->t - start)<<" G"<t)<<" (" << newPoint.x/1000.0 <<", "<x/1000.0) <<", " << (bestSP3->y/1000.0) <<", " << (bestSP3->z/1000.0) << ") "<clockBias << " " << getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first<< " " << dist.length()<< " "; + cout << (bestSP3->clockBias - getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first); + cout << " " << gs.af0 <<" " << gs.af1; + cout << endl; + + sp3csv <t << " 0 "<< sv <<" " << ephAge(gs.tow+(bestSP3->t - start), eph[sv].getT0e()) <<" "<x<<" " << bestSP3->y<<" " <z <<" " << newPoint.x<<" " <clockBias <<" "; + sp3csv << getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first<<" " << dist.length() <<" " << along <<" "; + sp3csv << (bestSP3->clockBias - getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first) << " " << E << " " << speed.length()<(latlonh)/M_PI <<" elev "<< std::get<2>(latlonh) << " acc "<(latlonh)/M_PI<<" "<< - 180*std::get<0>(latlonh)/M_PI<<" "<(latlonh)<<" "<(latlonh)/M_PI<<" "<< + // 180*std::get<0>(latlonh)/M_PI<<" "<(latlonh)<<" "< sbas((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().size()); + cout<<" PRN "< sbstate; + + if(type == 0) { + sbstate[nmm.sbm().gnsssv()].parse0(sbas, nmm.localutcseconds()); + } + else if(type == 1) { + sbstate[nmm.sbm().gnsssv()].parse1(sbas, nmm.localutcseconds()); + } + else if(type == 6) { + auto integ = sbstate[nmm.sbm().gnsssv()].parse6(sbas, nmm.localutcseconds()); + cout<<"integrity updated: "; + for(const auto& i : integ) { + cout<((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size())); @@ -932,13 +1032,13 @@ try const auto& eph = galEphemeris[a.sv]; Point sat; getCoordinates(rtow, eph, &sat); - elevA=getElevationDeg(sat, g_ourpos); + elevA=getElevationDeg(sat, g_srcpos[nmm.sourceid()]); } if(galEphemeris.count(b.sv)) { const auto& eph = galEphemeris[b.sv]; Point sat; getCoordinates(rtow, eph, &sat); - elevB=getElevationDeg(sat, g_ourpos); + elevB=getElevationDeg(sat, g_srcpos[nmm.sourceid()]); } return elevB < elevA; @@ -960,14 +1060,14 @@ try Point sat; double E=getCoordinates(rtow - clockoffms/1000.0, eph, &sat); - double range = Vector(g_ourpos, sat).length(); + double range = Vector(g_srcpos[nmm.sourceid()], sat).length(); getCoordinates(rtow - clockoffms/1000.0 - range/299792458.0, eph, &sat); - range = Vector(g_ourpos, sat).length(); + range = Vector(g_srcpos[nmm.sourceid()], sat).length(); double trmsec = ldexp(maxt - sv.tr, -32) + clockoffms; constexpr double omegaE = 2*M_PI /86164.091 ; - double rotcor = omegaE * (sat.x*g_ourpos.y - sat.y * g_ourpos.x) / 299792458.0; + double rotcor = omegaE * (sat.x*g_srcpos[nmm.sourceid()].y - sat.y * g_srcpos[nmm.sourceid()].x) / 299792458.0; range += rotcor; double bgdcor = 299792458.0 *ldexp(eph.BGDE1E5b,-32); @@ -996,7 +1096,7 @@ try cout<<" rotcor "<< rotcor; cout<<" relcor "< #include "influxpush.hh" +#include "sbas.hh" #include "CLI/CLI.hpp" #include "gpscnav.hh" @@ -63,19 +64,16 @@ struct ObserverPosition }; std::map g_srcpos; - -struct SBASStatus +struct SBASAndReceiverStatus { - time_t last_seen{0}; - time_t last_type_0{0}; + SBASState status; struct PerRecv { time_t last_seen{0}; }; map perrecv; - }; -typedef map sbas_t; +typedef map sbas_t; sbas_t g_sbas; GetterSetter g_sbaskeeper; @@ -95,158 +93,188 @@ struct GNSSReceiver Point position; }; +double g_GSTUTCOffset, g_GSTGPSOffset, g_GPSUTCOffset, g_BeiDouUTCOffset, g_GlonassUTCOffset, g_GlonassGPSOffset; + +/* +The situation. We have a single ephemeris function that is able to operate on +Galileo, GPS1 and Beidou structs. It does so using functions like getT0e(). +This ephemeris function also does speed and doppler. + +We have structs for all four GNSS. + +All GNSS have ephemerides that are spread out over multiple messages. +Because of that, we have to do some kind of atomic update. + +We also have an SVStat that has some knowledge about IODs. +It would be great is we could ask the svstat about "the latest ephemeris" +and get a useful answer. + +More concretely, we could ask the svstat about the latest *position* or *speed*. +It would be easier to do that. There is no unified "ephemeris object" between all the GNSS. + +We do need a unified "do we have a complete ephemeris method". + +For Galileo this is messages 1, 2, 3 and 4 +For GPS it is 2 and 3 +For BeiDou it is message 2 and message 3 in succession (?) + sow is in each message, forms a link +For GLONASS I don't really know + +We also care about ephemeris discontinuities. + +Idea is that SVStat has four structs around, one for each GNSS, each containing "the lastest complete ephemeris". + ephglomsg etc + + It also has infrastructure for storing the ephemerides as they are being assembled. + +Open question: + +*/ - - -void SVIOD::addGalileoWord(std::basic_string_view page) +// XXX conversion glonass?? +int SVStat::wn() const { - uint8_t wtype = getbitu(&page[0], 0, 6); - words[wtype]=true; - gnssid = 2; - if(wtype == 1) { - t0e = getbitu(&page[0], 16, 14) * 60; // WE SCALE THIS FOR THE USER! - m0 = getbits(&page[0], 30, 32); - e = getbitu(&page[0], 62, 32); - sqrtA = getbitu(&page[0], 94, 32); + if(gnss == 0) + return gpsmsg.wn; + else if(gnss == 2) + return galmsg.wn; + else if(gnss == 3) + return beidoumsg.wn; + else if(gnss == 6) { + uint32_t glotime = glonassMessage.getGloTime(); // this starts GLONASS time at 31st of december 1995, 00:00 UTC + return glotime / (7*86400); } - else if(wtype == 2) { - omega0 = getbits(&page[0], 16, 32); - i0 = getbits(&page[0], 48, 32); - omega = getbits(&page[0], 80, 32); - idot = getbits(&page[0], 112, 14); - } - else if(wtype == 3) { - omegadot = getbits(&page[0], 16, 24); - deltan = getbits(&page[0], 40, 16); - cuc = getbits(&page[0], 56, 16); - cus = getbits(&page[0], 72, 16); - crc = getbits(&page[0], 88, 16); - crs = getbits(&page[0], 104, 16); - sisa = getbitu(&page[0], 120, 8); - } - else if(wtype == 4) { - cic = getbits(&page[0], 22, 16); - cis = getbits(&page[0], 38, 16); - - t0c = getbitu(&page[0], 54, 14); - af0 = getbits(&page[0], 68, 31); - af1 = getbits(&page[0], 99, 21); - af2 = getbits(&page[0], 120, 6); - /* - cout<<(int) t0c << " " <<(int) af0 <<" " <<(int) af1 <<" " <<(int) af2<second; - throw std::runtime_error("Asked for live IOD, don't have one yet"); + if(gnss == 6) + return ::getCoordinates(tow, oldephglomsg, p); + + // getCoordinates needs to be overloaded for GLONASS + return ::getCoordinates(tow, prevIOD(), p, quiet); + } -void SVStat::checkCompleteAndClean(int iod) + +void SVStat::getSpeed(double tow, Vector* v) const { - if(iods[iod].complete()) { - for(const auto& i : iods) { - if(i.first != iod && i.second.complete()) - prevIOD=i; - } - SVIOD latest = iods[iod]; - - decltype(iods) newiods; // XXX race condition here, - newiods[iod]=latest; - iods.swap(newiods); // try to keep it brief - } + return ::getSpeed(tow, liveIOD(), v); } - -void SVStat::addGalileoWord(std::basic_string_view page) +DopplerData SVStat::doDoppler(double tow, const Point& us, double freq) const { - uint8_t wtype = getbitu(&page[0], 0, 6); - if(wtype == 0) { - if(getbitu(&page[0], 6,2) == 2) { - wn = getbitu(&page[0], 96, 12); - if(tow != getbitu(&page[0], 108, 20)) { - cerr<<"wtype "<=1 && wtype <= 4) { // ephemeris - uint16_t iod = getbitu(&page[0], 6, 10); - iods[iod].addGalileoWord(page); - checkCompleteAndClean(iod); - } - else if(wtype==5) { // disturbance, health, time - ai0 = getbitu(&page[0], 6, 11); - ai1 = getbits(&page[0], 17, 11); // ai1 & 2 are signed, 0 not - ai2 = getbits(&page[0], 28, 14); - - sf1 = getbitu(&page[0], 42, 1); - sf2 = getbitu(&page[0], 43, 1); - sf3 = getbitu(&page[0], 44, 1); - sf4 = getbitu(&page[0], 45, 1); - sf5 = getbitu(&page[0], 46, 1); - BGDE1E5a = getbits(&page[0], 47, 10); - BGDE1E5b = getbits(&page[0], 57, 10); - - e5bhs = getbitu(&page[0], 67, 2); - e1bhs = getbitu(&page[0], 69, 2); - e5bdvs = getbitu(&page[0], 71, 1); - e1bdvs = getbitu(&page[0], 72, 1); - - - wn = getbitu(&page[0], 73, 12); - if(tow != getbitu(&page[0], 85, 20)) - { - cerr<<"wtype "< ages; for(const auto& s: stats) if(s.first.gnss == (unsigned int)gnssid) - ages[7*s.second.wn*86400 + s.second.tow]= s.first; + ages[7*s.second.wn()*86400 + s.second.tow()]= s.first; if(ages.empty()) - throw runtime_error("Asked for latest WN for "+to_string(gnssid)+": we don't know it yet"); + throw runtime_error("Asked for latest WN for "+to_string(gnssid)+": we don't know it yet ("+to_string(stats.size())+")"); - return stats.find(ages.rbegin()->second)->second.wn; + return stats.find(ages.rbegin()->second)->second.wn(); } int latestTow(int gnssid, const svstats_t& stats) @@ -269,13 +297,14 @@ int latestTow(int gnssid, const svstats_t& stats) map ages; for(const auto& s: stats) if(s.first.gnss == (unsigned int) gnssid) - ages[7*s.second.wn*86400 + s.second.tow]= s.first; + ages[7*s.second.wn()*86400 + s.second.tow()]= s.first; if(ages.empty()) - throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet"); - return stats.find(ages.rbegin()->second)->second.tow; + throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet ("+to_string(stats.size())+")"); + return stats.find(ages.rbegin()->second)->second.tow(); } +// nanoseconds posix time from that gnss WN and TOW int64_t nanoTime(int gnssid, int wn, double tow) { int offset; @@ -295,9 +324,10 @@ int64_t nanoTime(int gnssid, int wn, double tow) return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); } +// same in seconds double satUTCTime(const SatID& id) { - return nanoTime(id.gnss, g_svstats[id].wn, g_svstats[id].tow)/1000000000.0; + return nanoTime(id.gnss, g_svstats[id].wn(), g_svstats[id].tow())/1000000000.0; } @@ -416,70 +446,57 @@ try auto svstats = g_statskeeper.get(); ret["leap-seconds"] = g_dtLS; try { + // if we haven't seen galileo, we have no idea ret["last-seen"]=utcFromGST(latestWN(2, svstats), latestTow(2, svstats)); } catch(...) {} - map utcstats, gpsgststats, gpsutcstats; + ret["gst-utc-offset-ns"] = g_GSTUTCOffset; + ret["gst-gps-offset-ns"] = g_GSTGPSOffset; + ret["gps-utc-offset-ns"] = g_GPSUTCOffset; + ret["beidou-utc-offset-ns"] = g_BeiDouUTCOffset; + ret["glonass-utc-offset-ns"] = g_GlonassUTCOffset; + ret["glonass-gps-offset-ns"] = g_GlonassGPSOffset; - for(const auto& s: svstats) { - if(!s.second.wn) // this will suck in 20 years - continue; - - //Galileo-UTC offset: 3.22 ns, Galileo-GPS offset: 7.67 ns, 18 leap seconds - - - if(s.first.gnss == 0) { // GPS - int dw = (uint8_t)s.second.wn - s.second.wn0t; - int age = dw * 7 * 86400 + s.second.tow - s.second.t0t; // t0t is PRESCALED // XXX changed this, - - gpsutcstats[age]=s.first; - continue; + map siggnsscount, svgnsscount; + map svcount, sigcount; + map receivers; + time_t now=time(0); + for(const auto& sv : svstats) { + bool fresh=false; + for(const auto& pr : sv.second.perrecv) { + int age = now - pr.second.t; + if(age < 30) { + fresh= true; + receivers[pr.first]++; + } } + if(fresh) { - int dw = (uint8_t)s.second.wn - s.second.wn0t; - int age = dw * 7 * 86400 + s.second.tow - s.second.t0t; // t0t is pre-scaled - utcstats[age]=s.first; + sigcount[sv.first]++; + siggnsscount[sv.first.gnss]++; + SatID id = sv.first; + id.sigid=0; + svcount[id]++; + } + } + for(const auto& sv : svcount) + svgnsscount[sv.first.gnss]++; - uint8_t wn0g = s.second.wn0t; - int dwg = (((uint8_t)s.second.wn)&(1+2+4+8+16+32)) - wn0g; - age = dwg*7*86400 + s.second.tow - s.second.t0g * 3600; - gpsgststats[age]=s.first; + ret["total-live-receivers"] = receivers.size(); + ret["total-live-svs"] = svcount.size(); + ret["total-live-signals"] = sigcount.size(); + ret["gps-svs"] = svgnsscount[0]; + ret["galileo-svs"] = svgnsscount[2]; + ret["beidou-svs"] = svgnsscount[3]; + ret["glonass-svs"] = svgnsscount[6]; - } - if(utcstats.empty()) { - ret["utc-offset-ns"]=nullptr; - } - else { - auto satid = utcstats.begin()->second; // freshest SV - long shift = svstats[{2,satid.sv,satid.sigid}].a0 * (1LL<<20) + svstats[{2,satid.sv,satid.sigid}].a1 * utcstats.begin()->first; // in 2^-50 seconds units - ret["utc-offset-ns"] = 1.073741824*ldexp(1.0*shift, -20); - ret["leap-second-planned"] = (svstats[satid].dtLSF != svstats[satid].dtLS); - } + ret["gps-sigs"] = siggnsscount[0]; + ret["galileo-sigs"] = siggnsscount[2]; + ret["beidou-sigs"] = siggnsscount[3]; + ret["glonass-sigs"] = siggnsscount[6]; - if(gpsgststats.empty()) { - ret["gps-offset-ns"]=nullptr; - } - else { - auto satid = gpsgststats.begin()->second; // freshest SV - long shift = svstats[{2,satid.sv, satid.sigid}].a0g * (1L<<16) + svstats[{2,satid.sv, satid.sigid}].a1g * gpsgststats.begin()->first; // in 2^-51 seconds units - - ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21); - } - - if(gpsutcstats.empty()) { - ret["gps-utc-offset-ns"]=nullptr; - } - else { - auto satid = gpsutcstats.begin()->second; // freshest SV - long shift = svstats[{0,satid.sv,satid.sigid}].a0 * (1LL<<20) + svstats[{0,satid.sv,satid.sigid}].a1 * gpsutcstats.begin()->first; // In 2^-50 seconds units - - ret["gps-utc-offset-ns"] = 1.073741824*ldexp(shift, -20); - } - - - return ret; }); @@ -510,7 +527,7 @@ try item["observed"]=false; if(auto iter = svstats.find({3, (uint32_t)ae.first, 0}); iter != svstats.end()) { - if(time(0) - nanoTime(3, iter->second.wn, iter->second.tow)/1000000000 < 300) + if(time(0) - nanoTime(3, iter->second.wn(), iter->second.tow())/1000000000 < 300) item["observed"] = true; } @@ -558,7 +575,7 @@ try item["observed"] = false; for(uint32_t sigid : {0,1,2}) { // XXX SIGIDS if(auto iter = svstats.find({6, (uint32_t)ae.first, sigid}); iter != svstats.end()) { - if(time(0) - nanoTime(6, iter->second.wn, iter->second.tow)/1000000000 < 300) { + if(time(0) - nanoTime(6, iter->second.wn(), iter->second.tow())/1000000000 < 300) { item["observed"] = true; auto longlat = getLongLat(iter->second.glonassMessage.x, iter->second.glonassMessage.y, iter->second.glonassMessage.z); item["eph-longitude"] = 180*longlat.first/M_PI; @@ -614,10 +631,10 @@ try if(auto iter = svstats.find({2, (uint32_t)ae.first, sigid}); iter != svstats.end()) { if(iter->second.completeIOD()) { - item["sisa"] = iter->second.liveIOD().sisa; + item["sisa"] = iter->second.galmsg.sisa; } // if we hit an 'observed', stop trying sigids - if(time(0) - nanoTime(2, iter->second.wn, iter->second.tow)/1000000000 < 300) { + if(time(0) - nanoTime(2, iter->second.wn(), iter->second.tow())/1000000000 < 300) { item["observed"] = true; break; } @@ -660,8 +677,8 @@ try item["e"] = ae.second.getE(); item["health"] = ae.second.health; item["t0e"] = ae.second.getT0e(); - item["t"]= ephAge(ae.second.getT0e(), latestTow(2, svstats))/86400.0; - item["eph-age"] = ephAge(latestTow(2, svstats), ae.second.getT0e()); + item["t"]= ephAge(ae.second.getT0e(), latestTow(0, svstats))/86400.0; + item["eph-age"] = ephAge(latestTow(0, svstats), ae.second.getT0e()); item["i0"] = 180.0 * ae.second.getI0()/ M_PI; item["inclination"] = 180 * ae.second.getI0() /M_PI; Point sat; @@ -677,7 +694,7 @@ try item["observed"] = false; for(uint32_t sigid : {0,1,4}) { if(auto iter = svstats.find({0, (uint32_t)ae.first, sigid}); iter != svstats.end()) { - if(time(0) - nanoTime(0, iter->second.wn, iter->second.tow)/1000000000 < 300) + if(time(0) - nanoTime(0, iter->second.wn(), iter->second.tow())/1000000000 < 300) item["observed"] = true; } } @@ -767,12 +784,12 @@ try if(hzCorrection) svo["delta_hz_corr"] = truncPrec(iter->second.deltaHz - *hzCorrection, 2); - getCoordinates(latestTow(sv.first.gnss, svstats), sv.second.liveIOD(), & sat); + sv.second.getCoordinates(latestTow(sv.first.gnss, svstats), & sat); } - if(sv.first.gnss == 3 && sv.second.oldBeidouMessage.sow >= 0 && sv.second.oldBeidouMessage.sqrtA != 0) { - getCoordinates(latestTow(sv.first.gnss, svstats), sv.second.oldBeidouMessage, &sat); + if(sv.first.gnss == 3 && sv.second.ephBeidoumsg.sow >= 0 && sv.second.ephBeidoumsg.sqrtA != 0) { + getCoordinates(latestTow(sv.first.gnss, svstats), sv.second.ephBeidoumsg, &sat); } - if(sv.first.gnss == 6 && sv.second.wn > 0) { + if(sv.first.gnss == 6 && sv.second.wn() > 0) { getCoordinates(latestTow(6, svstats), sv.second.glonassMessage, &sat); } if(sat.x) { @@ -837,36 +854,44 @@ try if(iter == svstats.end()) return ret; const auto& s= iter->second; + // XXX CONVERSION + /* ret["a0"] = s.a0; ret["a1"] = s.a1; ret["a0g"] = s.a0g; ret["a1g"] = s.a1g; + */ if(id.gnss == 2) { - ret["sf1"] = s.sf1; - ret["sf2"] = s.sf2; - ret["sf3"] = s.sf3; - ret["sf4"] = s.sf4; - ret["sf5"] = s.sf5; - ret["BGDE1E5a"] = s.BGDE1E5a; - ret["BGDE1E5b"] = s.BGDE1E5b; - ret["e5bdvs"]=s.e5bdvs; - ret["e1bdvs"]=s.e1bdvs; - ret["e5bhs"]=s.e5bhs; - ret["e1bhs"]=s.e1bhs; + ret["sf1"] = s.galmsg.sf1; + ret["sf2"] = s.galmsg.sf2; + ret["sf3"] = s.galmsg.sf3; + ret["sf4"] = s.galmsg.sf4; + ret["sf5"] = s.galmsg.sf5; + ret["BGDE1E5a"] = s.galmsg.BGDE1E5a; + ret["BGDE1E5b"] = s.galmsg.BGDE1E5b; + ret["e5bdvs"]=s.galmsg.e5bdvs; + ret["e1bdvs"]=s.galmsg.e1bdvs; + ret["e5bhs"]=s.galmsg.e5bhs; + ret["e1bhs"]=s.galmsg.e1bhs; } + // XXX CONVERSION + /* ret["ai0"] = s.ai0; ret["ai1"] = s.ai1; ret["ai2"] = s.ai2; - ret["wn"] = s.wn; - ret["tow"] = s.tow; + */ + ret["wn"] = s.wn(); + ret["tow"] = s.tow(); + // XXX CONVERSION + /* ret["dtLS"] = s.dtLS; ret["dtLSF"] = s.dtLSF; ret["wnLSF"] = s.wnLSF; ret["dn"] = s.dn; + */ - - if(id.gnss == 3 && svstats[id].oldBeidouMessage.sow >= 0 && svstats[id].oldBeidouMessage.sqrtA != 0) { - const auto& iod = svstats[id].oldBeidouMessage; + if(id.gnss == 3 && svstats[id].ephBeidoumsg.sow >= 0 && svstats[id].ephBeidoumsg.sqrtA != 0) { + const auto& iod = svstats[id].ephBeidoumsg; ret["e"] = iod.getE(); ret["omega"] = iod.getOmega(); ret["sqrtA"]= iod.getSqrtA(); @@ -889,7 +914,7 @@ try ret["af1"] = iod.a1; ret["af2"] = iod.a2; Point p; - getCoordinates(s.tow, iod, &p); + s.getCoordinates(s.tow(), &p); ret["x"] = p.x; ret["y"] = p.y; ret["z"] = p.z; @@ -900,7 +925,7 @@ try } else if(svstats[id].completeIOD()) { const auto& iod = svstats[id].liveIOD(); - ret["iod"]= svstats[id].getIOD(); + ret["iod"]= svstats[id].liveIOD().getIOD(); ret["e"] = iod.getE(); ret["omega"] = iod.getOmega(); ret["sqrtA"]= iod.getSqrtA(); @@ -911,19 +936,23 @@ try ret["omega0"] = iod.getOmega0(); ret["idot"] = iod.getIdot(); ret["t0e"] = iod.getT0e(); - ret["t0c"] = iod.getT0c(); + // XXX conversion + // ret["t0c"] = iod.getT0c(); ret["cuc"] = iod.getCuc(); ret["cus"] = iod.getCus(); ret["crc"] = iod.getCrc(); ret["crs"] = iod.getCrs(); ret["cic"] = iod.getCic(); ret["cis"] = iod.getCis(); + // XXX conversion + /* ret["sisa"] = iod.sisa; ret["af0"] = iod.af0; ret["af1"] = iod.af1; ret["af2"] = iod.af2; + */ Point p; - getCoordinates(s.tow, iod, &p); + s.getCoordinates(s.tow(), &p); ret["x"] = p.x; ret["y"] = p.y; ret["z"] = p.z; @@ -987,10 +1016,10 @@ try if(g.first < 0) continue; SatID id{2,(uint32_t)g.first,1}; - if(svstats[id].completeIOD() && svstats[id].liveIOD().sisa == 255) { + if(svstats[id].completeIOD() && svstats[id].galmsg.sisa == 255) { continue; } - if(svstats[id].e1bhs || svstats[id].e1bdvs) + if(svstats[id].galmsg.e1bhs || svstats[id].galmsg.e1bdvs) continue; sats.push_back(sat); } @@ -1003,11 +1032,11 @@ try if(g.first < 0) continue; SatID id{0,(uint32_t)g.first,0}; - if(svstats[id].completeIOD() && svstats[id].ura == 16) { + if(svstats[id].completeIOD() && svstats[id].gpsmsg.ura == 16) { // cout<<"Skipping G"< 0) { + if(s.first.gnss == 6 && s.second.wn() > 0) { Point sat; - getCoordinates(s.second.tow, s.second.glonassMessage, &sat); + getCoordinates(s.second.tow(), s.second.glonassMessage, &sat); if(svstats[s.first].glonassMessage.Bn & 7) { continue; } @@ -1102,13 +1131,14 @@ try for(const auto& s: sbas) { nlohmann::json item = nlohmann::json::object(); - item["last-seen"] = s.second.last_seen; - item["last-seen-s"] = time(0) - s.second.last_seen; + item["last-seen"] = s.second.status.d_lastSeen; + item["last-seen-s"] = time(0) - s.second.status.d_lastSeen; - item["last-type-0"] = s.second.last_type_0; - item["last-type-0-s"] = time(0) - s.second.last_type_0; + item["last-type-0"] = s.second.status.d_lastDNU; + item["last-type-0-s"] = time(0) - s.second.status.d_lastDNU; - if(s.second.last_seen - s.second.last_type_0 < 120) + // this 59 is to make sure galmonmon doesn't trigger on a single message + if(s.second.status.d_lastSeen - s.second.status.d_lastDNU < 59) item["health"]="DON'T USE"; else item["health"]="OK"; @@ -1119,13 +1149,8 @@ try perrecv[to_string(recv.first)]["last-seen-s"] = time(0) - recv.second.last_seen; } item["perrecv"]=perrecv; - - ret[to_string(s.first)]=item; - - - } return ret; }); @@ -1144,33 +1169,29 @@ try for(const auto& s: svstats) { nlohmann::json item = nlohmann::json::object(); - if(!s.second.tow) // I know, I know, will suck briefly + if(!s.second.tow()) // I know, I know, will suck briefly continue; item["gnssid"] = s.first.gnss; item["svid"] = s.first.sv; item["sigid"] = s.first.sigid; - // perhaps check oldBeidouMessage for sow >=0 as 'completeIOD'? + // perhaps check ephBeidoumsg for sow >=0 as 'completeIOD'? if(s.first.gnss == 3) { // beidou - item["sisa"]=humanUra(s.second.ura); - item["sisa-m"]=numUra(s.second.ura); - if(s.second.t0eMSB >= 0 && s.second.t0eLSB >=0) - item["eph-age-m"] = ephAge(s.second.tow, 8.0*((s.second.t0eMSB<<15) + s.second.t0eLSB))/60.0; + item["sisa"]=humanUra(s.second.ephBeidoumsg.urai); + item["sisa-m"]=numUra(s.second.ephBeidoumsg.urai); + if(s.second.completeIOD()) { + item["eph-age-m"] = ephAge(s.second.tow(), s.second.ephBeidoumsg.getT0e())/60.0; - if(s.second.tleMatch.distance >=0) { - item["best-tle"] = s.second.tleMatch.name; - item["best-tle-dist"] = s.second.tleMatch.distance /1000.0; - item["best-tle-norad"] = s.second.tleMatch.norad; - item["best-tle-int-desig"] = s.second.tleMatch.internat; } + Point p; - if(s.second.oldBeidouMessage.sqrtA != 0) { - getCoordinates(s.second.tow, s.second.oldBeidouMessage, &p); + if(s.second.ephBeidoumsg.sqrtA != 0) { + getCoordinates(s.second.tow(), s.second.ephBeidoumsg, &p); auto beidoualma = g_beidoualmakeeper.get(); if(auto iter = beidoualma.find(s.first.sv); iter != beidoualma.end()) { Point almapos; - getCoordinates(s.second.tow, iter->second.alma, &almapos); + getCoordinates(s.second.tow(), iter->second.alma, &almapos); item["alma-dist"] = truncPrec(Vector(almapos, p).length()/1000.0, 1); } } @@ -1180,50 +1201,54 @@ try item["sisa"] = humanFt(s.second.glonassMessage.FT); item["sisa-m"] = numFt(s.second.glonassMessage.FT); } - item["aode"] = s.second.aode; - item["iod"] = s.second.glonassMessage.Tb; + item["aode"] = s.second.ephglomsg.En*24; + item["iod"] = s.second.ephglomsg.Tb; - time_t glonow = nanoTime(6, s.second.wn, s.second.tow)/1000000000.0; + time_t glonow = nanoTime(6, s.second.wn(), s.second.tow())/1000000000.0; // the 820368000 stuff is to rebase to 'day 1' so the % works auto pseudoTow = (getGlonassT0e(glonow, s.second.glonassMessage.Tb) - 820368000) % (7*86400); - // cout<=0) { - item["best-tle"] = s.second.tleMatch.name; - item["best-tle-dist"] = truncPrec(s.second.tleMatch.distance /1000.0, 1); - item["best-tle-norad"] = s.second.tleMatch.norad; - item["best-tle-int-desig"] = s.second.tleMatch.internat; - } + // cout<second, &almapos); + getCoordinates(s.second.tow(), iter->second, &almapos); item["alma-dist"] = Vector(almapos, p).length()/1000.0; } } @@ -1248,35 +1273,39 @@ try auto galileoalma = g_galileoalmakeeper.get(); if(auto iter = galileoalma.find(s.first.sv); iter != galileoalma.end()) { Point almapos; - getCoordinates(s.second.tow, iter->second, &almapos); + getCoordinates(s.second.tow(), iter->second, &almapos); item["alma-dist"] = Vector(almapos, p).length()/1000.0; } } } - + // XX conversion + /* item["a0"]=s.second.a0; item["a1"]=s.second.a1; + */ if(s.first.gnss == 0) { // GPS - auto deltaUTC = getGPSUTCOffset(s.second.tow, s.second.wn, s.second); + auto deltaUTC = getGPSUTCOffset(s.second.tow(), s.second.wn(), s.second.gpsmsg); item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400); - item["t0t"] = s.second.t0t; - item["wn0t"] = s.second.wn0t; + + // CONVERSION + // item["t0t"] = s.second.gpsmsg.t0t; + // item["wn0t"] = s.second.gpsmsg.wn0t; } if(s.first.gnss == 2) { - auto deltaUTC = s.second.galmsg.getUTCOffset(s.second.tow, s.second.wn); + auto deltaUTC = s.second.galmsg.getUTCOffset(s.second.tow(), s.second.wn()); item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400); - auto deltaGPS = s.second.galmsg.getGPSOffset(s.second.tow, s.second.wn); + auto deltaGPS = s.second.galmsg.getGPSOffset(s.second.tow(), s.second.wn()); item["delta-gps"] = fmt::sprintf("%.1f %+.1f/d", deltaGPS.first, deltaGPS.second * 86400); item["t0t"] = s.second.galmsg.t0t; item["wn0t"] = s.second.galmsg.wn0t; } if(s.first.gnss == 3) { - auto deltaUTC = s.second.oldBeidouMessage.getUTCOffset(s.second.oldBeidouMessage.sow); + auto deltaUTC = s.second.ephBeidoumsg.getUTCOffset(s.second.ephBeidoumsg.sow); item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400); - auto deltaGPS = s.second.oldBeidouMessage.getGPSOffset(s.second.oldBeidouMessage.sow); + auto deltaGPS = s.second.ephBeidoumsg.getGPSOffset(s.second.ephBeidoumsg.sow); item["delta-gps"] = fmt::sprintf("%.1f %+.1f/d", deltaGPS.first, deltaGPS.second * 86400); item["t0g"] =0; item["wn0g"] = 0; @@ -1286,47 +1315,60 @@ try } - - item["dtLS"]=s.second.dtLS; + // XXX conversion + // item["dtLS"]=s.second.dtLS; if(s.first.gnss == 3) { // beidou - item["a0g"]=s.second.a0g; - item["a1g"]=s.second.a1g; - if(s.second.aode >= 0) - item["aode"]=s.second.aode; - if(s.second.aodc >= 0) - item["aodc"]=s.second.aodc; - + item["a0g"]=s.second.ephBeidoumsg.a0gps; + item["a1g"]=s.second.ephBeidoumsg.a1gps; + if(s.second.ephBeidoumsg.aode >= 0) + item["aode"]=s.second.ephBeidoumsg.aode; + if(s.second.ephBeidoumsg.aodc >= 0) + item["aodc"]=s.second.ephBeidoumsg.aodc; + + item["health"] = s.second.beidoumsg.sath1 ? "NOT OK" : "OK"; + item["healthissue"] = !!s.second.beidoumsg.sath1; } + if(s.first.gnss == 6) { // GLONASS + auto deltaUTC = s.second.glonassMessage.getUTCOffset(0); + item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400); + + auto deltaGPS = s.second.glonassMessage.getGPSOffset(0); + item["delta-gps"] = fmt::sprintf("%.1f %+.1f/d", deltaGPS.first, deltaGPS.second * 86400); + } if(s.first.gnss == 2) { // galileo - item["a0g"]=s.second.a0g; - item["a1g"]=s.second.a1g; - item["t0g"]=s.second.t0g; - item["wn0g"]=s.second.wn0g; + item["a0g"]=s.second.galmsg.a0g; + item["a1g"]=s.second.galmsg.a1g; + item["t0g"]=s.second.galmsg.t0g; + item["wn0g"]=s.second.galmsg.wn0g; item["health"] = - humanBhs(s.second.e1bhs) +"/" + - humanBhs(s.second.e5bhs) +"/" + - (s.second.e1bdvs ? "NG" : "val") +"/"+ - (s.second.e5bdvs ? "NG" : "val"); - item["e5bdvs"]=s.second.e5bdvs; - item["e1bdvs"]=s.second.e1bdvs; - item["e5bhs"]=s.second.e5bhs; - item["e1bhs"]=s.second.e1bhs; + humanBhs(s.second.galmsg.e1bhs) +"/" + + humanBhs(s.second.galmsg.e5bhs) +"/" + + (s.second.galmsg.e1bdvs ? "NG" : "val") +"/"+ + (s.second.galmsg.e5bdvs ? "NG" : "val"); + item["e5bdvs"]=s.second.galmsg.e5bdvs; + item["e1bdvs"]=s.second.galmsg.e1bdvs; + item["e5bhs"]=s.second.galmsg.e5bhs; + item["e1bhs"]=s.second.galmsg.e1bhs; item["healthissue"]=0; - if(s.second.e1bhs == 2 || s.second.e5bhs == 2) + if(s.second.galmsg.e1bhs == 2 || s.second.galmsg.e5bhs == 2) item["healthissue"] = 1; - if(s.second.e1bhs == 3 || s.second.e5bhs == 3) + if(s.second.galmsg.e1bhs == 3 || s.second.galmsg.e5bhs == 3) item["healthissue"] = 1; - if(s.second.e1bdvs || s.second.e5bdvs || s.second.e1bhs == 1 || s.second.e5bhs == 1) + if(s.second.galmsg.e1bdvs || s.second.galmsg.e5bdvs || s.second.galmsg.e1bhs == 1 || s.second.galmsg.e5bhs == 1) item["healthissue"] = 2; } - else if(s.first.gnss == 0 || s.first.gnss == 3 || s.first.gnss == 6) {// gps or beidou or GLONASS - item["health"]= s.second.gpshealth ? ("NOT OK: "+to_string(s.second.gpshealth)) : string("OK"); - item["healthissue"]= 2* !!s.second.gpshealth; + else if(s.first.gnss == 0) { + item["health"] =s.second.gpsmsg.gpshealth ? ("NOT OK: "+to_string(s.second.gpsmsg.gpshealth)) : string("OK"); + item["healthissue"]= 2* !!s.second.gpsmsg.gpshealth; + } + else if(s.first.gnss == 6) { // GLONASS + item["health"]= s.second.glonassMessage.Bn ? ("NOT OK: "+to_string(s.second.glonassMessage.Bn)) : string("OK"); + item["healthissue"]= 2* !!s.second.glonassMessage.Bn; } nlohmann::json perrecv = nlohmann::json::object(); @@ -1336,11 +1378,8 @@ try det["elev"] = pr.second.el; Point sat; - if((s.first.gnss == 0 || s.first.gnss == 2) && s.second.completeIOD()) - getCoordinates(latestTow(s.first.gnss, svstats), s.second.liveIOD(), & sat); - if(s.first.gnss == 3 && s.second.oldBeidouMessage.sow >= 0 && s.second.oldBeidouMessage.sqrtA != 0) { - getCoordinates(latestTow(s.first.gnss, svstats), s.second.oldBeidouMessage, &sat); - } + if(s.second.completeIOD()) + s.second.getCoordinates(latestTow(s.first.gnss, svstats), & sat); if(sat.x) { Point our = g_srcpos[pr.first].pos; @@ -1369,7 +1408,7 @@ try } item["perrecv"]=perrecv; - item["last-seen-s"] = time(0) - nanoTime(s.first.gnss, s.second.wn, s.second.tow)/1000000000; + item["last-seen-s"] = time(0) - nanoTime(s.first.gnss, s.second.wn(), s.second.tow())/1000000000; if(s.second.latestDisco >=0) { item["latest-disco"]= truncPrec(s.second.latestDisco, 3); @@ -1380,14 +1419,105 @@ try } - item["wn"] = s.second.wn; - item["tow"] = s.second.tow; + item["wn"] = s.second.wn(); + item["tow"] = s.second.tow(); item["fullName"] = makeSatIDName(s.first); item["name"] = makeSatPartialName(s.first); ret[makeSatIDName(s.first)] = item; } return ret; }); + + h2s.addHandler("/sbstatus.json", [](auto handler, auto req) { + addHeaders(req); + auto svstats = g_statskeeper.get(); + h2o_add_header(&req->pool, &req->res.headers, H2O_TOKEN_CACHE_CONTROL, + NULL, H2O_STRLIT("max-age=3")); + + // Access-Control-Allow-Origin + h2o_add_header(&req->pool, &req->res.headers, H2O_TOKEN_ACCESS_CONTROL_ALLOW_ORIGIN, + NULL, H2O_STRLIT("*")); + + auto ret = nlohmann::json::array(); + + time_t now = time(0); + for(const auto& s: svstats) { + for(const auto& sb : s.second.sbas) { + if(now - sb.second.longterm.lastUpdate > 120) + continue; + if(now - sb.second.fast.lastUpdate > 120) + continue; + if(sb.second.fast.udrei == 14) + continue; + + auto obj = nlohmann::json::object(); + obj["name"] = makeSatPartialName(s.first); + obj["sbasprn"] = sb.first; + + obj["dx"] = sb.second.longterm.dx; + obj["dy"] = sb.second.longterm.dy; + obj["dz"] = sb.second.longterm.dz; + obj["dai"] = sb.second.longterm.dai; + obj["iod8"] = sb.second.longterm.iod8; + obj["velocity"] = sb.second.longterm.velocity; + + obj["last-longterm-update-s"] = now - sb.second.longterm.lastUpdate; + obj["last-fast-update-s"] = now - sb.second.fast.lastUpdate; + + + if(sb.second.longterm.velocity) { + obj["toa"] = sb.second.longterm.toa; + int toadelta = (now % 86400) - sb.second.longterm.toa; + + obj["toa-delta"] = toadelta; + + obj["ddx"] = sb.second.longterm.ddx; + obj["ddy"] = sb.second.longterm.ddy; + obj["ddz"] = sb.second.longterm.ddz; + obj["ddai"] = sb.second.longterm.ddai; + + obj["adx"] = sb.second.longterm.dx + toadelta*sb.second.longterm.ddx; + obj["ady"] = sb.second.longterm.dy + toadelta*sb.second.longterm.ddy; + obj["adz"] = sb.second.longterm.dz + toadelta*sb.second.longterm.ddz; + obj["adai"] = sb.second.longterm.dai + toadelta*sb.second.longterm.ddai; + + + } + obj["correction"] = sb.second.fast.correction; + obj["udrei"] = sb.second.fast.udrei; + + if(s.second.completeIOD() && (s.second.liveIOD().getIOD() & 0xff) == sb.second.longterm.iod8) { + Point sat; + + s.second.getCoordinates(s.second.tow(), &sat); + Point adjsat=sat; + adjsat.x += sb.second.longterm.dx; + adjsat.y += sb.second.longterm.dy; + adjsat.z += sb.second.longterm.dz; + Point sbasCenter; + int prn = sb.first; + if(prn== 126 || prn == 136 || prn == 123) + sbasCenter = c_egnosCenter; + else if(prn == 138 || prn == 131 || prn == 133) + sbasCenter = c_waasCenter; + else + sbasCenter = Point{0,0,0}; + + double dist = Vector(sbasCenter, adjsat).length() - Vector(sbasCenter, sat).length(); + obj["space-shift"] = dist; + dist -= sb.second.longterm.dai / 3; + obj["eph-shift"] = dist; + obj["range-shift"] = dist - sb.second.fast.correction; + } + + + ret.push_back(obj); + } + } + return ret; + }); + + h2s.addDirectory("/", htmlDir); const char *address = localAddress.c_str(); @@ -1506,12 +1636,8 @@ try Point sat{0,0,0}; //cout<<"Got recdata for "< ["<< humanBhs(gm.e5bhs)<<", "<< humanBhs(gm.e1bhs)<<", "<< (int)gm.e5bdvs <<", " << (int)gm.e1bdvs<<"], lastseen "< ["<< humanBhs(gm.e5bhs)<<", "<< humanBhs(gm.e1bhs)<<", "<< (int)gm.e5bdvs <<", " << (int)gm.e1bdvs<<"], lastseen "< 1 && svstat.galmsgTyped[w-1].iodnav != svstat.galmsgTyped[w].iodnav) + break; + } + if(w==5) { + if(svstat.ephgalmsg.iodnav != svstat.galmsgTyped[1].iodnav) { + svstat.oldephgalmsg = svstat.ephgalmsg; + svstat.ephgalmsg = svstat.galmsgTyped[wtype]; + svstat.reportNewEphemeris(id, idb); + } + } + + } + + + // XXX conversion possibly vital + // g_svstats[id].tow = nmm.gi().gnsstow(); // g_svstats[id].perrecv[nmm.sourceid()].wn = nmm.gi().gnsswn(); // g_svstats[id].perrecv[nmm.sourceid()].tow = nmm.gi().gnsstow(); g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); - g_svstats[id].addGalileoWord(inav); - if(g_svstats[id].e1bhs || g_svstats[id].e5bhs || g_svstats[id].e1bdvs || g_svstats[id].e5bdvs) { - // if(sv != 18 && sv != 14) - // cout << "sv "<=1 && wtype <= 4) { // ephemeris - uint16_t iod = getbitu(&inav[0], 6, 10); - if(wtype == 3) { - idb.addValue(id, "sisa", {{"value", g_svstats[id].iods[iod].sisa}}, satUTCTime(id)); - } - else if(wtype == 4) { - idb.addValue(id, "clock", {{"offset_ns", svstat.galmsg.getAtomicOffset(svstat.tow).first}, - {"t0c", g_svstats[id].iods[iod].t0c*60}, - {"af0", g_svstats[id].iods[iod].af0}, - {"af1", g_svstats[id].iods[iod].af1}, - {"af2", g_svstats[id].iods[iod].af2}}, satUTCTime(id)); + + if(wtype >=1 && wtype <= 4) { // ephemeris + if(wtype == 3) { + idb.addValue(id, "sisa", {{"value", g_svstats[id].galmsg.sisa}}, satUTCTime(id)); + } + else if(wtype == 4) { + idb.addValue(id, "clock", {{"offset_ns", svstat.galmsg.getAtomicOffset(svstat.tow()).first}, + {"t0c", g_svstats[id].galmsg.t0c*60}, // getT0c()?? + {"af0", g_svstats[id].galmsg.af0}, + {"af1", g_svstats[id].galmsg.af1}, + {"af2", g_svstats[id].galmsg.af2}}, satUTCTime(id)); if(oldgm.af0 && oldgm.t0c != svstat.galmsg.t0c) { - auto oldOffset = oldgm.getAtomicOffset(svstat.tow); - auto newOffset = svstat.galmsg.getAtomicOffset(svstat.tow); + auto oldOffset = oldgm.getAtomicOffset(svstat.tow()); + auto newOffset = svstat.galmsg.getAtomicOffset(svstat.tow()); svstat.timeDisco = oldOffset.first - newOffset.first; if(fabs(svstat.timeDisco) < 10000) idb.addValue(id, "clock_jump_ns", {{"value", svstat.timeDisco}}, satUTCTime(id)); @@ -1593,41 +1737,39 @@ try } else if(wtype == 5) { idb.addValue(id, "iono", { - {"ai0", g_svstats[id].ai0}, - {"ai1", g_svstats[id].ai1}, - {"ai2", g_svstats[id].ai2}, - {"sf1", g_svstats[id].sf1}, - {"sf2", g_svstats[id].sf2}, - {"sf3", g_svstats[id].sf3}, - {"sf4", g_svstats[id].sf4}, - {"sf5", g_svstats[id].sf5}}, satUTCTime(id)); + {"ai0", g_svstats[id].galmsg.ai0}, + {"ai1", g_svstats[id].galmsg.ai1}, + {"ai2", g_svstats[id].galmsg.ai2}, + {"sf1", g_svstats[id].galmsg.sf1}, + {"sf2", g_svstats[id].galmsg.sf2}, + {"sf3", g_svstats[id].galmsg.sf3}, + {"sf4", g_svstats[id].galmsg.sf4}, + {"sf5", g_svstats[id].galmsg.sf5}}, satUTCTime(id)); idb.addValue(id, "galbgd", { - {"BGDE1E5a", g_svstats[id].BGDE1E5a}, - {"BGDE1E5b", g_svstats[id].BGDE1E5b}}, satUTCTime(id)); + {"BGDE1E5a", g_svstats[id].galmsg.BGDE1E5a}, + {"BGDE1E5b", g_svstats[id].galmsg.BGDE1E5b}}, satUTCTime(id)); idb.addValue(id, "galhealth", { - {"e1bhs", g_svstats[id].e1bhs}, - {"e5bhs", g_svstats[id].e5bhs}, - {"e5bdvs", g_svstats[id].e5bdvs}, - {"e1bdvs", g_svstats[id].e1bdvs}}, satUTCTime(id)); + {"e1bhs", g_svstats[id].galmsg.e1bhs}, + {"e5bhs", g_svstats[id].galmsg.e5bhs}, + {"e5bdvs", g_svstats[id].galmsg.e5bdvs}, + {"e1bdvs", g_svstats[id].galmsg.e1bdvs}}, satUTCTime(id)); } else if(wtype == 6) { // GST-UTC - int dw = (uint8_t)g_svstats[id].wn - g_svstats[id].wn0t; - int age = dw * 7 * 86400 + g_svstats[id].tow - g_svstats[id].t0t; // t0t is PRESCALED - - long shift = g_svstats[id].a0 * (1LL<<20) + g_svstats[id].a1 * age; // in 2^-50 seconds units + const auto& sv = g_svstats[id]; + g_GSTUTCOffset = sv.galmsg.getUTCOffset(sv.tow(), sv.wn()).first; idb.addValue(id, "utcoffset", { - {"a0", g_svstats[id].a0}, - {"a1", g_svstats[id].a1}, - {"t0t", g_svstats[id].t0t}, - {"delta", 1.073741824*ldexp(1.0*shift, -20)} + {"a0", sv.galmsg.a0}, + {"a1", sv.galmsg.a1}, + {"t0t", sv.galmsg.t0t}, + {"delta", g_GSTUTCOffset} }, satUTCTime(id)); - g_dtLS = g_svstats[id].dtLS; + g_dtLS = sv.galmsg.dtLS; } else if(wtype == 7) { // this contains first part of alma1 @@ -1655,66 +1797,26 @@ try // cout<<(int)wtype<<"b alma-sv "<= 0) { - int ephage = ephAge(ent.second.tow, ent.second.prevIOD.second.t0e); - if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) { - - cout<= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) { + if(id.gnss == 3 && g_svstats[id].ephBeidoumsg.sow >= 0 && g_svstats[id].ephBeidoumsg.sqrtA != 0) { double freq = 1561.098; if(nmm.rfd().sigid() != 0) freq = 1207.140; // the magic 14 is because 'rcvtow()' is in GPS/Galileo TOW // but BeiDou operates with 14 leap seconds less than GPS/Galileo - auto res = doDoppler(nmm.rfd().rcvtow()-14, g_srcpos[nmm.sourceid()].pos, g_svstats[id].oldBeidouMessage, freq * 1000000); + auto res = doDoppler(nmm.rfd().rcvtow()-14, g_srcpos[nmm.sourceid()].pos, g_svstats[id].ephBeidoumsg, freq * 1000000); if(isnan(res.preddop)) { cerr<<"Problem with doppler calculation for C"<((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size())); @@ -1975,71 +2078,92 @@ try g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); auto& svstat = g_svstats[id]; + svstat.gnss = id.gnss; auto oldsvstat = svstat; uint8_t page; - int frame=parseGPSMessage(cond, svstat, &page); + auto& gm = svstat.gpsmsg; + auto oldgm = gm; + int frame = gm.parseGPSMessage(cond, &page); + if(frame == 1) { - idb.addValue(id, "clock", {{"offset_ns", getGPSAtomicOffset(svstat.tow, svstat).first}, - {"t0c", 16.0*svstat.t0c}, - {"af0", 8.0*svstat.af0}, - {"af1", 8.0*svstat.af1}, - {"af2", 16.0*svstat.af2}}, satUTCTime(id)); + idb.addValue(id, "clock", {{"offset_ns", getGPSAtomicOffset(svstat.tow(), svstat.gpsmsg).first}, + {"t0c", 16.0*gm.t0c}, + {"af0", 8.0*gm.af0}, + {"af1", 8.0*gm.af1}, + {"af2", 16.0*gm.af2}}, satUTCTime(id)); - // cout<<"Got ura "< [" << humanUra(svstat.ura)<<"] "<<(int)svstat.ura<<", lastseen "< [" << humanUra(gm.ura)<<"] "<<(int)gm.ura<<", lastseen "< [" << (int)svstat.gpshealth <<"], lastseen "< [" << (int)gm.gpshealth <<"], lastseen "< ["<< (int)svstat.gpshealth<<"] , lastseen "< ["<< (int)gm.gpshealth<<"] , lastseen "<=25 && page<=32)) { - g_gpsalma[svstat.gpsalma.sv] = svstat.gpsalma; + g_gpsalma[gm.gpsalma.sv] = gm.gpsalma; } g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); - g_svstats[id].tow = nmm.gpsi().gnsstow(); - g_svstats[id].wn = nmm.gpsi().gnsswn(); - if(g_svstats[id].wn < 512) - g_svstats[id].wn += 2048; + // XXX conversion possibly vital + // g_svstats[id].tow = nmm.gpsi().gnsstow(); + // g_svstats[id].wn = nmm.gpsi().gnsswn(); + // if(g_svstats[id].wn < 512) // XXX ROLLOVER + // g_svstats[id].wn += 2048; } else if(nmm.type()== NavMonMessage::GPSCnavType) { SatID id{nmm.gpsc().gnssid(), nmm.gpsc().gnsssv(), nmm.gpsc().sigid()}; g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); auto& svstat = g_svstats[id]; + svstat.gnss = id.gnss; GPSCNavState gcns; parseGPSCNavMessage( @@ -2047,8 +2171,10 @@ try nmm.gpsc().contents().size()), gcns); // cout<<"Got a message from "<((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size())); - auto& bm = svstat.beidouMessage; + auto& bm = svstat.beidoumsg; auto oldbm = bm; int fraid=bm.parse(cond, &pageno); - svstat.tow = nmm.bid1().gnsstow(); - svstat.wn = nmm.bid1().gnsswn(); + // XXX conversion possibly vital + // svstat.tow = nmm.bid1().gnsstow(); + // svstat.wn = nmm.bid1().gnsswn(); if(fraid == 1) { - svstat.ura = bm.urai; - svstat.gpshealth = bm.sath1; - svstat.af0 = bm.a0; - svstat.af1 = bm.a1; - svstat.af2 = bm.a2; - svstat.aode = bm.aode; - svstat.aodc = bm.aodc; if(oldbm.sath1 != bm.sath1) { - cout<= 0) { - getCoordinates(svstat.tow, bm, &newpoint); - - if(fabs(svstat.lastTLELookupX - newpoint.x) > 300000) { - // cout<<"fraid 3 lookup, delta " << fabs(svstat.lastTLELookupX - newpoint.x) << endl; - auto match = g_tles.getBestMatch(nanoTime(3, svstat.wn, svstat.tow)/1000000000.0, newpoint.x, newpoint.y, newpoint.z); - svstat.tleMatch = match; - svstat.lastTLELookupX = newpoint.x; - } - - if(svstat.oldBeidouMessage.getT0e() != svstat.beidouMessage.getT0e()) { - getCoordinates(svstat.tow, svstat.oldBeidouMessage, &oldpoint); - Vector jump(oldpoint ,newpoint); - /* cout< (%f, %f, %f), jump: %f, seconds: %f\n", - id.second, oldpoint.x, oldpoint.y, oldpoint.z, - newpoint.x, newpoint.y, newpoint.z, jump.length(), (double)bm.getT0e() - svstat.oldBeidouMessage.getT0e()); - */ - double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600; - if(hours < 4) { - svstat.latestDisco = jump.length(); - } - else - svstat.latestDisco = -1; - - if(hours < 24) { - idb.addValue(id, "eph-disco", - {{"meters", jump.length()}, - {"seconds", hours*3600.0}, - {"oldx", oldpoint.x}, {"oldy", oldpoint.y}, {"oldz", oldpoint.z}, - {"x", newpoint.x}, {"y", newpoint.y}, {"z", newpoint.z}, - {"iod", 0}, - {"oldiod", 0}}, nanoTime(id.gnss, bm.wn, bm.sow)/1000000000.0); - } - } - + if(bm.sow - svstat.lastBeidouMessage2.sow == 6) { + if(svstat.ephBeidoumsg.getT0e() != svstat.beidoumsg.getT0e() && bm.sqrtA) { + svstat.oldephBeidoumsg = svstat.ephBeidoumsg; + svstat.ephBeidoumsg = bm; + svstat.reportNewEphemeris(id, idb); + } } - if(bm.sqrtA) // only copy in if complete - svstat.oldBeidouMessage = bm; } else if((fraid == 4 && 1<= pageno && pageno <= 24) || (fraid == 5 && 1<= pageno && pageno <= 6) || (fraid == 5 && 11<= pageno && pageno <= 23) ) { struct BeidouAlmanacEntry bae; - // bm.alma.AmEpID = svstat.oldBeidouMessage.alma.AmEpID; // this comes from older messages + // bm.alma.AmEpID = svstat.ephBeidoumsg.alma.AmEpID; // this comes from older messages if(processBeidouAlmanac(bm, bae)) { g_beidoualma[bae.sv]=bae; @@ -2154,14 +2242,19 @@ try } if(fraid==5 && pageno == 9) { + /* svstat.a0g = bm.a0gps; svstat.a1g = bm.a1gps; + */ } if(fraid==5 && pageno == 10) { + /* svstat.a0 = bm.a0utc; svstat.a1 = bm.a1utc; + */ g_dtLSBeidou = bm.deltaTLS; + g_BeiDouUTCOffset = bm.getUTCOffset(bm.sow).first; // cout<<"Beidou leap seconds: "<((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size())); g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); if(strno == 1 && gm.n4 != 0 && gm.NT !=0) { uint32_t glotime = gm.getGloTime(); // this starts GLONASS time at 31st of december 1995, 00:00 UTC - svstat.wn = glotime / (7*86400); - svstat.tow = glotime % (7*86400); + // CONVERSION, possibly vital + // svstat.wn = glotime / (7*86400); + // svstat.tow = glotime % (7*86400); // cout<<"Glonass now: "< 0) + if(svstat.wn() > 0) idb.addValue(id, "glohealth", {{"Bn", gm.Bn}}, satUTCTime(id)); if(oldgm.Bn != gm.Bn) { - cout< 0) { + // svstat.aode = gm.En * 24; + if(svstat.wn() > 0) { idb.addValue(id, "glo_taun_ns", {{"value", gm.getTaunNS()}}, satUTCTime(id)); idb.addValue(id, "ft", {{"value", gm.FT}}, satUTCTime(id)); @@ -2219,15 +2312,25 @@ try } } if(gm.x && gm.y && gm.z) { - time_t t0e = getGlonassT0e(nmm.localutcseconds(), gm.Tb); - - if(svstat.lastTLELookupX != gm.getX()) { - auto match = g_tles.getBestMatch(t0e, gm.getX(), gm.getY(), gm.getZ()); - svstat.tleMatch = match; - svstat.lastTLELookupX = gm.getX(); + if(svstat.ephglomsg.x != gm.x && + svstat.ephglomsg.y != gm.y && + svstat.ephglomsg.z != gm.z) { + svstat.oldephglomsg = svstat.ephglomsg; + svstat.ephglomsg = gm; + svstat.reportNewEphemeris(id, idb); } } } + else if(strno == 5) { + g_GlonassUTCOffset = gm.getUTCOffset(0).first; + g_GlonassGPSOffset = gm.getGPSOffset(0).first; + idb.addValue(id, "utcoffset", { + {"tauc", gm.tauc}, + {"delta", g_GlonassUTCOffset} + }, + satUTCTime(id)); + + } else if(strno == 6 || strno == 8 || strno == 10 || strno == 12 || strno == 14) { svstat.glonassAlmaEven = {nmm.localutcseconds(), gm}; } @@ -2240,13 +2343,18 @@ try } else if(nmm.type() == NavMonMessage::SBASMessageType) { auto& sb = g_sbas[nmm.sbm().gnsssv()]; - sb.last_seen = nmm.localutcseconds(); + sb.perrecv[nmm.sourceid()].last_seen = nmm.localutcseconds(); basic_string sbas((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().length()); - int type = getbitu(&sbas[0], 8, 6); - if(type == 0) { // the don't use message - sb.last_type_0 = nmm.localutcseconds(); + sb.status.parse(sbas, nmm.localutcseconds()); + if(nmm.localutcseconds() - sb.status.d_lastDNU > 120) { + for(const auto& c : sb.status.d_fast) { + g_svstats[c.first].sbas[nmm.sbm().gnsssv()].fast = c.second; + } + for(const auto& c : sb.status.d_longterm) { + g_svstats[c.first].sbas[nmm.sbm().gnsssv()].longterm = c.second; + } } } else { diff --git a/navparse.hh b/navparse.hh index 76156f0..f774185 100644 --- a/navparse.hh +++ b/navparse.hh @@ -6,6 +6,9 @@ #include "glonass.hh" #include #include "tle.hh" +#include "sbas.hh" +#include "ephemeris.hh" + using namespace std; // XXX struct SVPerRecv @@ -18,128 +21,53 @@ struct SVPerRecv int qi{-1}; // quality indicator, -1 = unknown time_t t; // last seen }; - -struct SVIOD -{ - std::bitset<32> words; - int gnssid; - uint32_t t0e; - uint32_t e, sqrtA; - int32_t m0, omega0, i0, omega, idot, omegadot, deltan; - - int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0}; - // 60 seconds - uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS - - // 2^-34 2^-46 - int32_t af0{0} , af1{0}; - // 2^-59 - int8_t af2{0}; - - uint8_t sisa; - - uint32_t wn{0}, tow{0}; - bool complete() const - { - if(gnssid==2) - return words[1] && words[2] && words[3] && words[4]; - else - return words[2] && words[3]; - } - void addGalileoWord(std::basic_string_view page); - - double getMu() const - { - if(gnssid == 2) return 3.986004418 * pow(10.0, 14.0); - if(gnssid == 0) return 3.986005 * pow(10.0, 14.0); - throw std::runtime_error("getMu() called for unsupported gnssid "+to_string(gnssid)); - } // m^3/s^2 - // same for galileo & gps - double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s - - uint32_t getT0e() const { return t0e; } - uint32_t getT0c() const { return 60*t0c; } - double getSqrtA() const { return ldexp(sqrtA, -19); } - double getE() const { return ldexp(e, -33); } - double getCuc() const { return ldexp(cuc, -29); } // radians - double getCus() const { return ldexp(cus, -29); } // radians - double getCrc() const { return ldexp(crc, -5); } // meters - double getCrs() const { return ldexp(crs, -5); } // meters - double getM0() const { return ldexp(m0 * M_PI, -31); } // radians - double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s - double getI0() const { return ldexp(i0 * M_PI, -31); } // radians - double getCic() const { return ldexp(cic, -29); } // radians - double getCis() const { return ldexp(cis, -29); } // radians - double getOmegadot() const { return ldexp(omegadot * M_PI, -43); } // radians/s - double getOmega0() const { return ldexp(omega0 * M_PI, -31); } // radians - double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s - double getOmega() const { return ldexp(omega * M_PI, -31); } // radians - - -}; - - -/* Most of thes fields are raw, EXCEPT: - t0t = seconds, raw fields are 3600 seconds (galileo), 4096 seconds (GPS) -*/ +class InfluxPusher; struct SVStat { - uint8_t e5bhs{0}, e1bhs{0}; - uint8_t gpshealth{0}; - uint16_t ai0{0}; - int16_t ai1{0}, ai2{0}; - bool sf1{0}, sf2{0}, sf3{0}, sf4{0}, sf5{0}; - int BGDE1E5a{0}, BGDE1E5b{0}; - bool e5bdvs{false}, e1bdvs{false}; - - uint16_t wn{0}; // we put the "unrolled" week number here! - uint32_t tow{0}; // "last seen" - // - // 2^-30 2^-50 1 8-bit week - int32_t a0{0}, a1{0}, t0t{0}, wn0t{0}; - int32_t a0g{0}, a1g{0}, t0g{0}, wn0g{0}; - int8_t dtLS{0}, dtLSF{0}; - uint16_t wnLSF{0}; - uint8_t dn; // leap second day number - // 1 2^-31 2^-43 2^-55 16 second - int ura, af0, af1, af2, t0c, gpsiod; // GPS parameters that should not be here XXX (or perhaps they should) + int gnss; + GPSState gpsmsg; // continuously being updated + GPSState gpsmsg2, gpsmsg3; // new ephemeris being assembled here + GPSState ephgpsmsg, oldephgpsmsg; // always has a consistent ephemeris GPSAlmanac gpsalma; - - // beidou: - int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1}; - BeidouMessage beidouMessage, oldBeidouMessage; - BeidouMessage lastBeidouMessage1, lastBeidouMessage2; + + int wn() const; // gets from the 'live' message + int tow() const; // same + TLERepo::Match tleMatch; double lastTLELookupX{0}; + // live, ephemeris + BeidouMessage beidoumsg, ephBeidoumsg, oldephBeidoumsg; + // internal + BeidouMessage lastBeidouMessage1, lastBeidouMessage2; + // new galileo - GalileoMessage galmsg; + // consistent, live + GalileoMessage ephgalmsg, galmsg, oldephgalmsg; + // internal map galmsgTyped; // Glonass - GlonassMessage glonassMessage; + GlonassMessage ephglomsg, glonassMessage, oldephglomsg; pair glonassAlmaEven; map perrecv; double latestDisco{-1}, latestDiscoAge{-1}, timeDisco{-1000}; - - map iods; - void addGalileoWord(std::basic_string_view page); - - bool completeIOD() const; - uint16_t getIOD() const; - SVIOD liveIOD() const; - SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor - pair prevIOD{-1, SVIOD()}; - void clearPrev() - { - prevIOD.first = -1; - } - void checkCompleteAndClean(int iod); + map sbas; + + const GPSLikeEphemeris& liveIOD() const; + const GPSLikeEphemeris& prevIOD() const; + + bool completeIOD() const; + double getCoordinates(double tow, Point* p, bool quiet=true) const; + double getOldEphCoordinates(double tow, Point* p, bool quiet=true) const; + void getSpeed(double tow, Vector* v) const; + DopplerData doDoppler(double tow, const Point& us, double freq) const; + void reportNewEphemeris(const SatID& id, InfluxPusher& idb); }; diff --git a/sbas.hh b/sbas.hh index 7adc1b2..8481015 100644 --- a/sbas.hh +++ b/sbas.hh @@ -4,7 +4,7 @@ #include #include "navmon.hh" #include - +#include "minivec.hh" // 0 do not use // 1 PRN mask @@ -17,6 +17,11 @@ // 26 Ionospheric delay corrections // 27 SBAS service message +// GSA HQ Prague +const Point c_egnosCenter{3970085, 1021937, 4869792}; + +// Somewhere in Minnesota, Dakota, Canada border +const Point c_waasCenter{-510062, -4166466, 4786089}; struct SBASState {