From dbd807b268467be95364bcd085cfa7e525ceb5af Mon Sep 17 00:00:00 2001 From: bert hubert Date: Fri, 30 Aug 2019 17:56:41 +0200 Subject: [PATCH] lots --- beidou.hh | 33 ++-- ephemeris.cc | 2 + ephemeris.hh | 36 ++++ galileo.hh | 33 +++- glonass.hh | 24 ++- gps.hh | 28 ++- html/doalles.js | 11 +- minicurl.cc | 4 +- navdump.cc | 56 +++++- navmon.proto | 12 +- navparse.cc | 284 ++++++++++++++++++++--------- navrecv.cc | 3 +- storage.cc | 2 +- ubxdisplay.cc | 467 ------------------------------------------------ ubxtool.cc | 23 ++- 15 files changed, 424 insertions(+), 594 deletions(-) delete mode 100644 ubxdisplay.cc diff --git a/beidou.hh b/beidou.hh index d144fbd..d4b3964 100644 --- a/beidou.hh +++ b/beidou.hh @@ -4,6 +4,7 @@ #include "bits.hh" #include #include +#include "ephemeris.hh" std::basic_string getCondensedBeidouMessage(std::basic_string_view payload); int beidouBitconv(int their); @@ -16,9 +17,6 @@ int beidouBitconv(int their); C05 (58.75E) */ - - - struct BeidouMessage { uint8_t strtype; @@ -70,11 +68,30 @@ struct BeidouMessage uint8_t sath1, aodc, urai, aode; uint16_t t0c; - int wn{-1}, a0, a1, a2; + // 2^-33 2^-50 2^-66 + int wn{-1}, a0, a1, a2; + // 2^17 2^30 + + uint32_t getT0c() + { + return 8 * t0c; + } + std::pair getAtomicOffset(int Sow = -1) + { + if(Sow == -1) + Sow = sow; + int delta = ephAge(Sow, getT0c()); + double cur = a0 + ldexp(delta*a1, -17) + ldexp(delta*delta*a2, -30); + double trend = ldexp(a1, -17) + ldexp(2*delta*a2, -30); + + // now in units of 2^-33 seconds, which are ~0.116 nanoseconds each + double factor = ldexp(1000000000, -33); + return {factor * cur, factor * trend}; + } void parse1(std::basic_string_view cond) { - sath1 = bbitu(43,1 ); + sath1 = bbitu(43,1); aodc = bbitu(31+13, 5); urai = bbitu(31+12+1+5, 4); wn = bbitu(61, 13); @@ -86,7 +103,7 @@ struct BeidouMessage } int t0eMSB{-1}; - uint32_t sqrtA, e; + uint32_t sqrtA{0}, e; int32_t deltan; int32_t cuc, cus, crc, crs; int32_t m0; @@ -113,10 +130,7 @@ struct BeidouMessage crc = bbits(199, 18); crs = bbits(225, 18); sqrtA = bbitu(251, 32); - t0eMSB = bbitu(291, 2); - - } int t0eLSB{-1}; @@ -143,7 +157,6 @@ struct BeidouMessage idot = bbits(190, 14); Omega0 = bbits(212, 32); omega = bbits(252, 32); - } // 4 is all almanac diff --git a/ephemeris.cc b/ephemeris.cc index 3f626a2..1ec731b 100644 --- a/ephemeris.cc +++ b/ephemeris.cc @@ -10,6 +10,7 @@ | tow t0e | -> 7*86400 - tow + t0e < 3.5 days, ok */ +// positive age = t0e in the past int ephAge(int tow, int t0e) { unsigned int diff; @@ -29,3 +30,4 @@ int ephAge(int tow, int t0e) return tow - t0e; // in the future, negative age } } + diff --git a/ephemeris.hh b/ephemeris.hh index 02f001b..f19aa54 100644 --- a/ephemeris.hh +++ b/ephemeris.hh @@ -148,3 +148,39 @@ void getCoordinates(int wn, double tow, const T& iod, Point* p, bool quiet=true) } +struct DopplerData +{ + double preddop; + double radvel; + Vector speed; + Point sat; + double ephage; + time_t t; +}; + +template +DopplerData doDoppler(int wn, int tow, const Point& us, const T& eph, double freq) +{ + DopplerData ret; + + // be careful with time here - we need to evaluate at the timestamp of this RFDataType update + // which might be newer than .tow in g_svstats + getCoordinates(wn, tow, eph, &ret.sat); + Point core; + Vector us2sat(us, ret.sat); + getSpeed(wn, tow, eph, &ret.speed); + Vector core2us(core, us); + Vector dx(us, ret.sat); // = x-ourx, dy = y-oury, dz = z-ourz; + + us2sat.norm(); + ret.radvel=us2sat.inner(ret.speed); + double c=299792458; + ret.preddop = -freq * ret.radvel/c; + + // be careful with time here - + ret.ephage = ephAge(tow, eph.getT0e()); + // cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "< #include #include +#include "ephemeris.hh" +#include "bits.hh" bool getTOWFromInav(std::basic_string_view inav, uint32_t *satTOW, uint16_t *wn); @@ -80,12 +82,12 @@ struct GalileoMessage 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 + uint16_t t0c; // 2^-34 2^-46 - int32_t af0{-1} , af1{-1}; + int32_t af0{0} , af1{0}; // 2^-59 - int8_t af2{-1}; + int8_t af2{0}; uint8_t sisa; @@ -132,6 +134,23 @@ struct GalileoMessage sisa = getbitu(&page[0], 120, 8); } + int getT0c() + { + return t0c * 60; + } + + std::pair getAtomicOffset(int tow) + { + int delta = ephAge(tow, getT0c()); + double cur = af0 + ldexp(delta*af1, -12) + ldexp(delta*delta*af2, -25); + double trend = ldexp(af1, -12) + ldexp(2*delta*af2, -25); + + // now in units of 2^-34 seconds, which are ~0.058 nanoseconds each + double factor = ldexp(1000000000, -34); + return {factor * cur, factor * trend}; + } + + // can't get enough of that ephemeris void parse4(std::basic_string_view page) { @@ -139,10 +158,10 @@ struct GalileoMessage 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); + t0c = getbitu(&page[0], 54, 14); // 60 + af0 = getbits(&page[0], 68, 31); // 2^-34 + af1 = getbits(&page[0], 99, 21); // 2^-46 + af2 = getbits(&page[0], 120, 6); // 2^-59 } // ionospheric disturbance, health, group delay, time diff --git a/glonass.hh b/glonass.hh index c1a788c..fbef419 100644 --- a/glonass.hh +++ b/glonass.hh @@ -28,7 +28,8 @@ struct GlonassMessage else if(strtype == 5) { parse5(gstr); } - + else if(strtype == 6 || strtype ==8 || strtype == 10 || strtype ==12 ||strtype ==14) + parse6_8_10_12_14(gstr); return strtype; } @@ -92,9 +93,14 @@ struct GlonassMessage */ uint16_t NT; - uint8_t FT, En, deltaTaun, M; - uint32_t taun; + uint8_t FT{255}, En, deltaTaun, M; + int32_t taun; // 2^-30 bool P4; + + double getTaunNS() + { + return 1000*ldexp(1000000*taun, -30); + } void parse4(std::basic_string_view gstr) { @@ -104,7 +110,7 @@ struct GlonassMessage taun = getbitsglonass(&gstr[0], 85-80, 22); En = getbitu(&gstr[0], 85-53, 5); P4 = getbitu(&gstr[0], 85-34, 1); - // missing delta tau n + deltaTaun = getbitsglonass(&gstr[0], 85 - 58, 4); } uint8_t n4; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in @@ -113,9 +119,17 @@ struct GlonassMessage { n4=getbitu(&gstr[0], 85-36, 5); taugps = getbitsglonass(&gstr[0], 85-31, 22); - } + int nA; + bool CnA; + + + void parse6_8_10_12_14(std::basic_string_view gstr) + { + CnA = getbitu(&gstr[0], 85-80, 1); + nA = getbitu(&gstr[0], 85-77, 5); + } }; diff --git a/gps.hh b/gps.hh index 0540ed6..6b30d79 100644 --- a/gps.hh +++ b/gps.hh @@ -4,6 +4,7 @@ #include #include "bits.hh" #include +#include std::basic_string getCondensedGPSMessage(std::basic_string_view payload); struct GPSState @@ -17,12 +18,12 @@ struct GPSState 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 + // 16 seconds + uint16_t t0c; - // 2^-34 2^-46 + // 2^-31 2^-43 int32_t af0 , af1; - // 2^-59 + // ??? int8_t af2; uint32_t wn{0}, tow{0}; }; @@ -51,6 +52,25 @@ struct GPSState void checkCompleteAndClean(int iod){} }; +template +int getT0c(const T& eph) +{ + return eph.t0c * 16; +} + +template +std::pair getAtomicOffset(int tow, const T& eph) +{ + int delta = ephAge(tow, getT0c(eph)); + double cur = eph.af0 + ldexp(delta*eph.af1, -12) + ldexp(delta*delta*eph.af2, -24); + double trend = ldexp(eph.af1, -12) + ldexp(2*delta*eph.af2, -24); + + // now in units of 2^-31 seconds, which are 0.5 nanoseconds each + double factor = ldexp(1000000000, -31); + return {factor * cur, factor * trend}; +} + + // 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) diff --git a/html/doalles.js b/html/doalles.js index f721d8f..c6cef17 100644 --- a/html/doalles.js +++ b/html/doalles.js @@ -14,7 +14,7 @@ function maketable(str, arr) enter(). append("tr"); - var columns = ["sv", "iod", "aode", "eph-age-m", "latest-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"]; + var columns = ["sv", "iod", "aodc/e", "eph-age-m", "latest-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"]; // append the header row thead.append("tr") @@ -50,8 +50,13 @@ function maketable(str, arr) // ret.value=""; ret.value += " "+row.sv; } - else - if(column == "eph-age-m") { + else if(column == "aodc/e") { + if(row["aodc"] != null && row["aode"] != null) + ret.value = row["aodc"]+"/"+row["aode"]; + else + ret.value=""; + } + else if(column == "eph-age-m") { if(row[column] != null) { var b = moment.duration(-row[column], 'm'); ret.value = b.humanize(true); diff --git a/minicurl.cc b/minicurl.cc index c6308f5..a62409f 100644 --- a/minicurl.cc +++ b/minicurl.cc @@ -111,7 +111,7 @@ void MiniCurl::setupURL(const std::string& str, const ComboAddress* rem, const C curl_easy_setopt(d_curl, CURLOPT_PROTOCOLS, CURLPROTO_HTTP | CURLPROTO_HTTPS); curl_easy_setopt(d_curl, CURLOPT_SSL_VERIFYPEER, false); curl_easy_setopt(d_curl, CURLOPT_SSL_VERIFYHOST, false); - curl_easy_setopt(d_curl, CURLOPT_FAILONERROR, true); + // curl_easy_setopt(d_curl, CURLOPT_FAILONERROR, true); curl_easy_setopt(d_curl, CURLOPT_URL, str.c_str()); curl_easy_setopt(d_curl, CURLOPT_WRITEFUNCTION, write_callback); curl_easy_setopt(d_curl, CURLOPT_WRITEDATA, this); @@ -148,7 +148,7 @@ std::string MiniCurl::postURL(const std::string& str, const std::string& postdat long http_code = 0; curl_easy_getinfo(d_curl, CURLINFO_RESPONSE_CODE, &http_code); - if(res != CURLE_OK) { + if(res != CURLE_OK || http_code >= 300 ) { cerr<<"Detailed error: "< inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size()); static map gms; + static map oldgm4s; GalileoMessage& gm = gms[nmm.gi().gnsssv()]; int wtype = gm.parse(inav); cout << "gal inav for "< oldgs1s; + + cout << "gpshealth = "<<(int)gs.gpshealth<<", wn "<second.t0c != gs.t0c) { + auto oldOffset = getAtomicOffset(gs.tow, iter->second); + auto newOffset = getAtomicOffset(gs.tow, gs); + cout<<" Timejump: "<second) )<<" seconds, old t0c "<second.t0c; + } + oldgs1s[sv] = gs; } else if(frame == 2) { cout << "t0e = "<second.t0e << " " <second.t0e); @@ -146,7 +169,16 @@ int main(int argc, char** argv) int fraid = bm.parse(cond, &pageno); cout<<"BeiDou "< msgs; + if(msgs[sv].wn>= 0 && msgs[sv].t0c != bm.t0c) { + auto oldOffset = msgs[sv].getAtomicOffset(bm.sow); + auto newOffset = bm.getAtomicOffset(bm.sow); + cout<<" Timejump: "<((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size())); + + cout<<"Glonass R"< using namespace std; struct EofException{}; @@ -35,6 +35,29 @@ struct GNSSReceiver int g_dtLS{18}; +uint64_t utcFromGST(int wn, int tow) +{ + return (935280000 + wn * 7*86400 + tow - g_dtLS); +} + +double utcFromGST(int wn, double tow) +{ + return (935280000.0 + wn * 7*86400 + tow - g_dtLS); +} + +double utcFromGPS(int wn, double tow) +{ + return (315964800 + wn * 7*86400 + tow - g_dtLS); +} + + +string humanFt(uint8_t ft) +{ + static const char* ret[]={"100 cm", "200 cm", "250 cm", "400 cm", "500 cm", "7 m", "10 m", "12 m", "14 m", "16 m", "32 m", "64 m", "128 m", "256 m", "512 m", "NONE"}; + if(ft < 16) + return ret[ft]; + return "???"; +} string humanSisa(uint8_t sisa) @@ -76,9 +99,9 @@ struct SVIOD uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS // 2^-34 2^-46 - int32_t af0 , af1; + int32_t af0{0} , af1{0}; // 2^-59 - int8_t af2; + int8_t af2{0}; uint8_t sisa; @@ -197,13 +220,19 @@ struct SVStat int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX // beidou: - int t0eMSB{-1}, t0eLSB{-1}, aode{-1}; + int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1}; BeidouMessage beidouMessage, oldBeidouMessage; - BeidouMessage lastBeidouMessage2; + BeidouMessage lastBeidouMessage1, lastBeidouMessage2; + + // new galileo + GalileoMessage galmsg; + + // Glonass + GlonassMessage glonassMessage; map perrecv; pair deltaHz; - double latestDisco{-1}; + double latestDisco{-1}, timeDisco{-1000}; map iods; void addGalileoWord(std::basic_string_view page); @@ -323,11 +352,12 @@ void SVStat::addGalileoWord(std::basic_string_view page) } } -void getSpeed(int wn, double tow, const SVIOD& iod, Vector* v) +template +void getSpeed(int wn, double tow, const T& eph, Vector* v) { Point a, b; - getCoordinates(wn, tow-0.5, iod, &a); - getCoordinates(wn, tow+0.5, iod, &b); + getCoordinates(wn, tow-0.5, eph, &a); + getCoordinates(wn, tow+0.5, eph, &b); *v = Vector(a, b); } @@ -351,7 +381,7 @@ int latestTow(int gnssid) if(s.first.first == gnssid) ages[7*s.second.wn*86400 + s.second.tow]= s.first; if(ages.empty()) - throw runtime_error("Asked for latest WN: we don't know it yet"); + throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet"); return g_svstats[ages.rbegin()->second].tow; } @@ -366,25 +396,12 @@ uint64_t nanoTime(int gnssid, int wn, int tow) offset = 935280000; if(gnssid == 3) // Beidou, 01-01-2006 offset = 1136073600; - if(gnssid == 6) // GLONASS - throw std::runtime_error("GLONASS does not have WN/TOW"); + if(gnssid == 6) { // GLONASS + offset = 820368000; + return 1000000000ULL*(offset + wn * 7*86400 + tow); // no leap seconds in glonass + } - return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); // Leap!! -} - -uint64_t utcFromGST(int wn, int tow) -{ - return (935280000 + wn * 7*86400 + tow - g_dtLS); -} - -double utcFromGST(int wn, double tow) -{ - return (935280000.0 + wn * 7*86400 + tow - g_dtLS); -} - -double utcFromGPS(int wn, double tow) -{ - return (315964800 + wn * 7*86400 + tow - g_dtLS); + return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); } @@ -409,8 +426,7 @@ struct InfluxPusher { if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0) return; - if(id.first == 3) - cout << g_svstats[id].wn <<", "< " < " <=0 as 'completeIOD'? + + + if(s.first.first == 3) { // beidou item["sisa"]=humanUra(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; + + if(time(0) - s.second.deltaHz.first < 60) { + item["delta_hz"] = s.second.deltaHz.second; + if(hzCorrection) + item["delta_hz_corr"] = s.second.deltaHz.second - (1561.098/1575.42)* (*hzCorrection); + } + + } + else if(s.first.first == 6) { // glonass + if(s.second.glonassMessage.FT < 16) + item["sisa"] = humanFt(s.second.glonassMessage.FT); + item["aode"] = s.second.aode; + item["iod"] = s.second.glonassMessage.Tb; } if(s.second.completeIOD()) { item["iod"]=s.second.getIOD(); @@ -644,7 +676,7 @@ try Point core; // this should actually use local time! - getCoordinates(latestWN(2), latestTow(2), s.second.liveIOD(), &p); + getCoordinates(0, s.second.tow, s.second.liveIOD(), &p); Vector core2us(core, our); Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz; @@ -672,8 +704,11 @@ try if(s.first.first == 3) { // beidou item["a0g"]=s.second.a0g; item["a1g"]=s.second.a1g; - if(s.second.aode > 0) + if(s.second.aode >= 0) item["aode"]=s.second.aode; + if(s.second.aodc >= 0) + item["aodc"]=s.second.aodc; + } @@ -699,26 +734,30 @@ try item["healthissue"] = 2; } - else if(s.first.first == 0 || s.first.first == 3) {// gps or beidou + else if(s.first.first == 0 || s.first.first == 3 || s.first.first == 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; } nlohmann::json perrecv = nlohmann::json::object(); for(const auto& pr : s.second.perrecv) { - nlohmann::json det = nlohmann::json::object(); - det["elev"] = pr.second.el; - det["db"] = pr.second.db; - det["last-seen-s"] = time(0) - pr.second.t; - perrecv[to_string(pr.first)]=det; + if(pr.second.el > 0 && pr.second.el <= 90) { + nlohmann::json det = nlohmann::json::object(); + det["elev"] = pr.second.el; + det["db"] = pr.second.db; + det["last-seen-s"] = time(0) - pr.second.t; + perrecv[to_string(pr.first)]=det; + } } item["perrecv"]=perrecv; - // xxx this is silly, should use local time - item["last-seen-s"] = s.second.tow ? (7*86400*(latestWN(s.first.first) - s.second.wn) + latestTow(s.first.first) - (int)s.second.tow) : -1; + item["last-seen-s"] = time(0) - nanoTime(s.first.first, s.second.wn, s.second.tow)/1000000000; if(s.second.latestDisco >=0) { item["latest-disco"]= s.second.latestDisco; } + if(s.second.timeDisco > -100 && s.second.timeDisco < 100) { + item["time-disco"]= s.second.timeDisco; + } item["wn"] = s.second.wn; @@ -779,8 +818,10 @@ try int sv = nmm.gi().gnsssv(); pair id={2,sv}; g_svstats[id].wn = nmm.gi().gnsswn(); - - unsigned int wtype = getbitu(&inav[0], 0, 6); + auto& svstat = g_svstats[id]; + auto oldgm = svstat.galmsg; + + unsigned int wtype = svstat.galmsg.parse(inav); if(1) { // cout< nmm.gi().gnsstow()) { @@ -818,6 +859,14 @@ try double offset = ldexp(1000.0*(1.0*g_svstats[id].iods[iod].af0 + ldexp(age*g_svstats[id].iods[iod].af1, -12)), -34); idb.addValue(id, "atomic_offset_ns", 1000000.0*offset); + + if(oldgm.af0 && oldgm.t0c != svstat.galmsg.t0c) { + auto oldOffset = oldgm.getAtomicOffset(svstat.tow); + auto newOffset = svstat.galmsg.getAtomicOffset(svstat.tow); + svstat.timeDisco = oldOffset.first - newOffset.first; + idb.addValue(id, "clock_jump_ns", svstat.timeDisco); + } + } else ; @@ -940,44 +989,43 @@ try else if(nmm.type() == NavMonMessage::RFDataType) { int sv = nmm.rfd().gnsssv(); pair id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()}; - if(g_svstats[id].completeIOD()) { - Point sat; - Point us=g_ourpos; + if(id.first == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) { + auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].oldBeidouMessage, 1561.098 * 1000000); - // be careful with time here - we need to evaluate at the timestamp of this RFDataType update - // which might be newer than .tow in g_svstats - getCoordinates(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_svstats[id].liveIOD(), &sat); - Point core; - Vector us2sat(us, sat); - Vector speed; - getSpeed(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_svstats[id].liveIOD(), &speed); - // cout< 10) { + + g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop}; + idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop); + auto corr = getHzCorrection(t); + if(corr) { + idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - (1561.098/1575.42) * (*corr)); + } + } + + } + else if(g_svstats[id].completeIOD()) { + auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].liveIOD(), 1575.42 * 1000000); + time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow()); + dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " < 10) { - g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - preddop}; - idb.addValue(id, "delta_hz", nmm.rfd().doppler() - preddop); + g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop}; + idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop); auto corr = getHzCorrection(t); if(corr) { - idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - preddop - *corr); + idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - *corr); } } @@ -991,6 +1039,7 @@ try g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); auto& svstat = g_svstats[id]; + auto oldsvstat = svstat; uint8_t page; int frame=parseGPSMessage(cond, svstat, &page); if(frame == 1) { @@ -1005,6 +1054,13 @@ try double offset = ldexp(1000.0*(1.0*g_svstats[id].af0 + ldexp(age*g_svstats[id].af1, -12)), -31); idb.addValue(id, "atomic_offset_ns", 1000000.0*offset); + + if(oldsvstat.af0 && oldsvstat.t0c != svstat.t0c) { + auto oldOffset = getAtomicOffset(svstat.tow, oldsvstat); + auto newOffset = getAtomicOffset(svstat.tow, svstat); + svstat.timeDisco = oldOffset.first - newOffset.first; + idb.addValue(id, "clock_jump_ns", svstat.timeDisco); + } } else if(frame==2) { idb.addValue(id, "gpshealth", g_svstats[id].gpshealth); @@ -1025,19 +1081,19 @@ try if(g_svstats[id].wn < 512) g_svstats[id].wn += 2048; } - else if(nmm.type()== NavMonMessage::BeidouInavType) { - pair id{nmm.bi().gnssid(), nmm.bi().gnsssv()}; + else if(nmm.type()== NavMonMessage::BeidouInavTypeD1) { + pair id{nmm.bid1().gnssid(), nmm.bid1().gnsssv()}; g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); auto& svstat = g_svstats[id]; uint8_t pageno; - auto cond = getCondensedBeidouMessage(std::basic_string((uint8_t*)nmm.bi().contents().c_str(), nmm.bi().contents().size())); + auto cond = getCondensedBeidouMessage(std::basic_string((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size())); auto& bm = svstat.beidouMessage; int fraid=bm.parse(cond, &pageno); - svstat.tow = nmm.bi().gnsstow(); - svstat.wn = nmm.bi().gnsswn(); + svstat.tow = nmm.bid1().gnsstow(); + svstat.wn = nmm.bid1().gnsswn(); if(fraid == 1) { svstat.ura = bm.urai; svstat.gpshealth = bm.sath1; @@ -1045,6 +1101,20 @@ try svstat.af1 = bm.a1; svstat.af2 = bm.a2; svstat.aode = bm.aode; + svstat.aodc = bm.aodc; + idb.addValue(id, "atomic_offset_ns", 1000000.0*bm.getAtomicOffset().first); + idb.addValue(id, "t0c", bm.getT0c()); + idb.addValue(id, "af0", bm.a0 * 2); + idb.addValue(id, "af1", bm.a1 / 16); + idb.addValue(id, "af2", bm.a2 / 128); // scaled to galileo units + if(svstat.lastBeidouMessage1.wn >=0 && svstat.lastBeidouMessage1.t0c != bm.t0c) { + auto oldOffset = svstat.lastBeidouMessage1.getAtomicOffset(bm.sow); + auto newOffset = bm.getAtomicOffset(bm.sow); + svstat.timeDisco = oldOffset.first - newOffset.first; + idb.addValue(id, "clock_jump_ns", svstat.timeDisco); + + } + svstat.lastBeidouMessage1 = bm; } if(fraid == 2) { svstat.lastBeidouMessage2 = bm; @@ -1060,9 +1130,16 @@ try 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()); - svstat.latestDisco = jump.length(); + double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600; + if(hours < 4) { + svstat.latestDisco = jump.length(); + idb.addValue(id, "eph-disco", jump.length()); + } + else + svstat.latestDisco = -1; } - svstat.oldBeidouMessage = bm; + if(bm.sqrtA) // only copy in if complete + svstat.oldBeidouMessage = bm; } if(fraid==5 && pageno == 9) { svstat.a0g = bm.a0gps; @@ -1077,7 +1154,52 @@ try getCoordinates(svstat.wn, svstat.tow, bm, &sat); Vector l(core, sat); - cout<<"C"<((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size())); + int fraid = getbitu(&cond[0], beidouBitconv(16), 3); + int sow = getbitu(&cond[0], beidouBitconv(19), 20); + int pnum = getbitu(&cond[0], beidouBitconv(43), 4); + int pre = getbitu(&cond[0], beidouBitconv(1), 11); + (void) pre; + // cout<<"C"<< nmm.bid2().gnsssv() << " sent D2 message, pre "< id{nmm.gloi().gnssid(), nmm.gloi().gnsssv()}; + auto& svstat = g_svstats[id]; + + auto& gm = svstat.glonassMessage; + int strno = gm.parse(std::basic_string((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size())); + g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds(); + if(strno == 1) { + struct tm tm; + memset(&tm, 0, sizeof(tm)); + tm.tm_year = 119; + tm.tm_mon = 8 - 1; + tm.tm_mday = 30; + tm.tm_hour = gm.hour - 3; + tm.tm_min = gm.minute; + tm.tm_sec = gm.seconds; + + time_t t = timegm(&tm); + uint32_t glotime = t - 820368000; // this starts GLONASS time at 31st of december 1995, 00:00 UTC + svstat.wn = glotime / (7*86400); + svstat.tow = glotime % (7*86400); + + } + if(strno == 2) { + svstat.gpshealth = gm.Bn; + } + if(strno == 4) { + svstat.aode = gm.En * 24; + idb.addValue(id, "glo_taun_ns", gm.getTaunNS()); + idb.addValue(id, "ft", gm.FT); + } + cout<<"GLONASS R"< getPathComponents(std::string_view root, time_t s, uint64_t sourc ret.push_back(to_string(tm.tm_year+1900)); ret.push_back(to_string(tm.tm_mon+1)); - ret.push_back(to_string(tm.tm_mday+1)); + ret.push_back(to_string(tm.tm_mday)); ret.push_back(to_string(tm.tm_hour)+".pb"); return ret; } diff --git a/ubxdisplay.cc b/ubxdisplay.cc deleted file mode 100644 index e1274ed..0000000 --- a/ubxdisplay.cc +++ /dev/null @@ -1,467 +0,0 @@ -#include -#include -#include -#include -#include "fmt/format.h" -#include "fmt/printf.h" -#include -#include -#include -#include -#include -#include -using namespace std; - - -struct EofException{}; - -uint8_t getUint8() -{ - int c; - c = fgetc(stdin); - if(c == -1) - throw EofException(); - return (uint8_t) c; -} - -uint16_t getUint16() -{ - uint16_t ret; - int res = fread(&ret, 1, sizeof(ret), stdin); - if(res != sizeof(ret)) - throw EofException(); - // ret = ntohs(ret); - return ret; -} - -/* lovingly lifted from RTKLIB */ -unsigned int getbitu(const unsigned char *buff, int pos, int len) -{ - unsigned int bits=0; - int i; - for (i=pos;i>(7-i%8))&1u); - return bits; -} - -extern int getbits(const unsigned char *buff, int pos, int len) -{ - unsigned int bits=getbitu(buff,pos,len); - if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits; - return (int)(bits|(~0u< str) -{ - uint8_t CK_A = 0, CK_B = 0; - - auto update = [&CK_A, &CK_B](uint8_t c) { - CK_A = CK_A + c; - CK_B = CK_B + CK_A; - }; - update(ubxClass); - update(ubxType); - uint16_t len = str.size(); - update(((uint8_t*)&len)[0]); - update(((uint8_t*)&len)[1]); - for(unsigned int I=0; I < str.size(); I++) { - update(str[I]); - } - return (CK_B << 8) + CK_A; -} - -struct SVIOD -{ - std::bitset<32> words; - uint32_t t0e; - uint8_t sisa; - bool complete() const - { - return words[1] && words[3]; - } - void addWord(std::basic_string_view page); -}; - -void SVIOD::addWord(std::basic_string_view page) -{ - uint8_t wtype = getbitu(&page[0], 0, 6); - words[wtype]=true; - if(wtype == 1) { - t0e = 60*getbitu(&page[0], 16, 14); - } - else if(wtype == 3) { - sisa = getbitu(&page[0], 120, 8); - } -} - -struct SVStat -{ - uint8_t e5bhs{0}, e1bhs{0}; - bool e5bdvs{false}, e1bdvs{false}; - bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false}; - uint16_t wn{0}; - uint32_t tow{0}; // "last seen" - uint32_t a0{0}, a1{0}; - int el{0}, db{0}; - map iods; - void addWord(std::basic_string_view page); - bool completeIOD() const; - uint16_t getIOD() const; - SVIOD liveIOD() const; -}; - -bool SVStat::completeIOD() const -{ - for(const auto& iod : iods) - if(iod.second.complete()) - return true; - return false; -} - -uint16_t SVStat::getIOD() const -{ - for(const auto& iod : iods) - if(iod.second.complete()) - return iod.first; - throw std::runtime_error("Asked for unknown IOD"); -} - -SVIOD SVStat::liveIOD() const -{ - if(auto iter = iods.find(getIOD()); iter != iods.end()) - return iter->second; - throw std::runtime_error("Asked for unknown IOD"); -} - -void SVStat::addWord(std::basic_string_view page) -{ - uint8_t wtype = getbitu(&page[0], 0, 6); - if(wtype >=1 && wtype <= 4) { // ephemeris - uint16_t iod = getbitu(&page[0], 6, 10); - iods[iod].addWord(page); - if(iods[iod].complete()) { - SVIOD latest = iods[iod]; - iods.clear(); - iods[iod] = latest; - } - } - else if(wtype==5) { // disturbance, health, time - 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); - tow = getbitu(&page[0], 85, 20); - // cout<<"Setting tow to "< d_windows; - static int d_h, d_w; - void emitLine(int sv, std::string_view line); - void setStatus(int sv, std::string_view line); -}; - -int WinKeeper::d_h; -int WinKeeper::d_w; - -WinKeeper::WinKeeper() -{ - initscr(); - getmaxyx(stdscr, d_h, d_w); - - int winwidth = d_w / 7; - for(int n=0; n < 8 ; ++n) { - d_windows.push_back({ - newwin(3, winwidth, 0, n*(winwidth+2)), - newwin(d_h-3, winwidth, 3, n*(winwidth+2)), - 0}); - scrollok(d_windows[n].text, 1); - } -}; - -void WinKeeper::emitLine(int sv, std::string_view line) -{ - for(auto& w: d_windows) { - if(w.sv == sv) { - w.emitLine(line); - return; - } - } - // nothing matched - for(auto& w: d_windows) { - if(!w.sv) { - w.sv = sv; - w.setHeader(std::to_string(sv)); - w.emitLine(line); - return; - } - } - throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv)); -} - -void WinKeeper::setStatus(int sv, std::string_view line) -{ - for(auto& w: d_windows) { - if(w.sv == sv) { - w.setStatus(line); - return; - } - } - // nothing matched - for(auto& w: d_windows) { - if(!w.sv) { - w.sv = sv; - w.setHeader(std::to_string(sv)); - w.setStatus(line); - return; - } - } - throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv)); -} - - -std::map g_svstats; - -int main() -try -{ - WinKeeper wk; - - // unsigned int tow=0, wn=0; - ofstream csv("iod.csv"); - ofstream csv2("toe.csv"); - csv<<"timestamp sv iod sisa"< strs; - boost::split(strs,line,boost::is_any_of(",")); - for(unsigned int n=4; n + 4 < strs.size(); n += 4) { - int sv = atoi(strs[n].c_str()); - - g_svstats[sv].el = atoi(strs[n+1].c_str()); - g_svstats[sv].db = atoi(strs[n+3].c_str()); - - } - } - line.clear(); - } - continue; - } - c = getUint8(); - if(c != 0x62) { - ungetc(c, stdin); // might be 0xb5 - continue; - } - - // if we are here, just had ubx header - - uint8_t ubxClass = getUint8(); - uint8_t ubxType = getUint8(); - uint16_t msgLen = getUint16(); - - // cout <<"Had an ubx message of class "<<(int) ubxClass <<" and type "<< (int) ubxType << " of " << msgLen <<" bytes"< msg; - msg.reserve(msgLen); - for(int n=0; n < msgLen; ++n) - msg.append(1, getUint8()); - - uint16_t ubxChecksum = getUint16(); - if(ubxChecksum != calcUbxChecksum(ubxClass, ubxType, msg)) { - cout<<"Checksum: "< payload; - for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i) - for(int j=1; j <= 4; ++j) - payload.append(1, msg[8 + (i+1) * 4 -j]); - - // for(auto& c : payload) - // fmt::printf("%02x ", c); - - // cout<<"\n"; - - std::basic_string inav; - unsigned int i,j; - for (i=0,j=2; i<14; i++, j+=8) - inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8)); - for (i=0,j=2; i< 2; i++, j+=8) - inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8)); - - // cout<<"inav for "<=1 && wtype <= 4) { // ephemeris - uint16_t iod = getbitu(&inav[0], 6, 10); - if(wtype == 1 && tow) { - int t0e = 60*getbitu(&inav[0], 16, 14); - int age = (tow - t0e)/60; - uint32_t e = getbitu(&inav[0], 6+10+14+32, 32); - wk.emitLine(sv, "Eph" +std::to_string(wtype)+", e=" + to_string(e)+", age="+to_string(age)+"m" ); - } - else if(wtype == 3) { - unsigned int sisa = getbitu(&inav[0], 120, 8); - wk.emitLine(sv, "Eph3, iod="+to_string(iod)+", sisa="+to_string(sisa)); - } - else - wk.emitLine(sv, "Eph" +std::to_string(wtype)+", iod=" + to_string(iod)); - } - else if(!wtype) { - wk.emitLine(sv, "."); - if(getbitu(&inav[0], 6,2) == 2) { - wn = getbitu(&inav[0], 96, 12); - tow = getbitu(&inav[0], 108, 20); - } - } - else if(wtype == 5) { - string out="IonoBGDHealth "; - for(int n=0; n <5; ++n) - out.append(1, getbitu(&inav[0], 42+n, 1) ? '1' : '0'); - out+=" "; - out += to_string(getbitu(&inav[0], 67,2))+to_string(getbitu(&inav[0], 69,2))+to_string(getbitu(&inav[0], 70,1))+to_string(getbitu(&inav[0], 71,1)); - wk.emitLine(sv, out); - wk.setStatus(sv, "Hlth: "+std::to_string(getbitu(&inav[0], 67, 2)) +", el="+to_string(g_svstats[sv].el)+", db="+to_string(g_svstats[sv].db) ); - tow = getbitu(&inav[0], 85, 20); - wn = getbitu(&inav[0], 73, 12); - } - else if(wtype == 6) { - int a0 = getbits(&inav[0], 6, 32); - int a1 = getbits(&inav[0], 38, 24); - int t0t = getbitu(&inav[0], 70, 8); - uint8_t wn0t = getbits(&inav[0], 78, 8); - int dw = (uint8_t)wn - wn0t; - if(tow && wn) - wk.emitLine(sv, "GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1)+", age="+to_string(tow/3600-t0t)+"h, dw="+to_string(dw) - +", wn0t="+to_string(wn0t)+", wn8="+to_string(wn&0xff)); - else - wk.emitLine(sv, "GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1)); - - } - else if(wtype >= 7 && wtype <= 9) { - uint16_t ioda = getbitu(&inav[0], 6, 4); - wk.emitLine(sv, "Alm"+to_string(wtype)+" IODa="+to_string(ioda)); - } - else if(wtype == 10) { - int a0g = getbits(&inav[0], 86, 16); - int a1g = getbits(&inav[0], 102, 12); - int t0g = getbitu(&inav[0], 114, 8); - uint8_t wn0g = getbitu(&inav[0], 122, 6); - int dw = (((uint8_t)wn)&(1+2+4+8+16+32)) - wn0g; - - if(tow && wn) { - time_t t = 935280000 + wn * 7*86400 + tow; - struct tm tm; - gmtime_r(&t, &tm); - - int age = tow - t0g * 3600; - // a0g = 2^-32 s, a1 = 2^-50 s/s - - // int shift = a0g * (1U<<16) + a1g * age; // in 2^-51 seconds units - - wk.emitLine(sv, "GST-GPS, a0g="+to_string(a0g)+", a1g="+to_string(a1g)+", t0g="+to_string(t0g)+", age="+to_string(tow/3600-t0g)+"h, dw="+to_string(dw) - +", wn0g="+to_string(wn0g)+", wn6="+to_string(wn&(1+2+4+8+16+32))); - } - - } - - else - wk.emitLine(sv, "Word "+std::to_string(wtype)); - - // time_t t = 935280000 + wn * 7*86400 + tow; - /* - for(const auto& ent : g_svstats) { - // 12 iod t0e - fmt::printf("%2d\t", ent.first); - if(ent.second.completeIOD()) { - cout << ent.second.getIOD() << "\t" << ( ent.second.tow - ent.second.liveIOD().t0e ) << "\t" << (unsigned int)ent.second.liveIOD().sisa; - } - cout<<"\n"; - } - cout<<"\n"; - */ - } - } - } - catch(EofException& e) - {} diff --git a/ubxtool.cc b/ubxtool.cc index aeab628..6f913c5 100644 --- a/ubxtool.cc +++ b/ubxtool.cc @@ -432,7 +432,7 @@ int main(int argc, char** argv) // GAL min max res x1 x2 x3, x4 0x02, 0x04, 0x08, 0, doGalileo, 0x00, 0x01, 0x00, // GLO min max res x1 x2 x3, x4 - 0x06, 0x04, 0x08, 0, doGlonass, 0x00, 0x01, 0x00 + 0x06, 0x06, 0x08, 0, doGlonass, 0x00, 0x01, 0x00 }); @@ -779,14 +779,23 @@ int main(int argc, char** argv) continue; } else if(id.first==6) { - cerr<<"SFRBX from GLONASS "< gms; GlonassMessage& gm = gms[id.second]; int strno = gm.parse(gstr); - cerr<<"R"< ("< " << (int)gm.Tb<<", P1: "<<(int)gm.P1<set_freq(payload[3]); + nmm.mutable_gloi()->set_gnssid(id.first); + nmm.mutable_gloi()->set_gnsssv(id.second); + nmm.mutable_gloi()->set_contents(string((char*)gstr.c_str(), gstr.size())); + emitNMM(1, nmm); + } } else cerr<<"SFRBX from unsupported GNSSID "<