From 28479b4fafe285b8d1e4f46408864102627f0442 Mon Sep 17 00:00:00 2001 From: bert hubert Date: Fri, 9 Aug 2019 15:58:52 +0200 Subject: [PATCH] move to protobuf --- Makefile | 13 +- README.md | 7 + galileo.cc | 26 ++ galileo.hh | 5 + navmon.proto | 53 +++ navparse.cc | 902 +++++++++++++++++++++++++++++++++++++++++++++++++++ ubx.cc | 29 ++ ubx.hh | 4 + ubxparse.cc | 413 +++++++++++------------ ubxtool.cc | 524 ++++++++++++++++++++++-------- 10 files changed, 1618 insertions(+), 358 deletions(-) create mode 100644 galileo.cc create mode 100644 galileo.hh create mode 100644 navmon.proto create mode 100644 navparse.cc diff --git a/Makefile b/Makefile index 12f1f2f..1c3c80d 100644 --- a/Makefile +++ b/Makefile @@ -1,6 +1,6 @@ CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ -PROGRAMS = ubxparse ubxdisplay minread ubxtool +PROGRAMS = ubxparse navparse ubxdisplay minread ubxtool all: $(PROGRAMS) @@ -14,9 +14,16 @@ SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simpl ubxparse: ubxparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o g++ -std=gnu++17 $^ -o $@ -pthread -lncurses -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl # -lwslay +navparse: navparse.o ext/fmt-5.2.1/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o + g++ -std=gnu++17 $^ -o $@ -pthread -lncurses -L/usr/local/lib -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf # -lwslay + + +navmon.pb.h: navmon.proto + protoc --cpp_out=./ navmon.proto + ubxdisplay: ubxdisplay.o ext/fmt-5.2.1/src/format.o g++ -std=gnu++17 $^ -o $@ -pthread -lncurses -ubxtool: ubxtool.o ubx.o ext/fmt-5.2.1/src/format.o - g++ -std=gnu++17 $^ -o $@ +ubxtool: ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o navmon.pb.o + g++ -std=gnu++17 $^ -o $@ -lprotobuf diff --git a/README.md b/README.md index cca3497..4f4e683 100644 --- a/README.md +++ b/README.md @@ -32,3 +32,10 @@ The magic value is there to help us resync from partially written data. The whole goal is that we can continue to rebuild the database by rerunning 'navstore' and 'navinflux'. +ubxtool +------- + * Will also spool raw serial data to disk (in a filename that includes the + start date) + * Can also read from disk + * Careful to add the right timestamps + diff --git a/galileo.cc b/galileo.cc new file mode 100644 index 0000000..8830217 --- /dev/null +++ b/galileo.cc @@ -0,0 +1,26 @@ +#include "bits.hh" +#include "galileo.hh" + +bool getTOWFromInav(std::basic_string_view inav, uint32_t *satTOW, uint16_t *wn) +{ + unsigned int wtype = getbitu(&inav[0], 0, 6); + if(wtype==0) { + if(getbitu(&inav[0], 6,2) == 2) { + *wn = getbitu(&inav[0], 96, 12); + *satTOW = getbitu(&inav[0], 108, 20); + return true; + } + } + else if(wtype==5) { + *wn = getbitu(&inav[0], 73, 12); + *satTOW = getbitu(&inav[0], 85, 20); + return true; + } + else if(wtype==6) { + // !! NO WN!! + *satTOW=getbitu(&inav[0], 105, 20); + return true; + } + + return false; +} diff --git a/galileo.hh b/galileo.hh new file mode 100644 index 0000000..c072572 --- /dev/null +++ b/galileo.hh @@ -0,0 +1,5 @@ +#pragma once +#include +#include + +bool getTOWFromInav(std::basic_string_view inav, uint32_t *satTOW, uint16_t *wn); diff --git a/navmon.proto b/navmon.proto new file mode 100644 index 0000000..a517f51 --- /dev/null +++ b/navmon.proto @@ -0,0 +1,53 @@ +syntax = "proto2"; + +message NavMonMessage { + enum Type { + ReceptionDataType = 1; + ObserverPositionType = 2; + GalileoInavType = 3; + RFDataType = 4; + } + + required uint64 sourceID = 1; + + required Type type = 2; + required uint64 localUtcSeconds = 3; + required uint64 localUtcNanoseconds = 4; + + message GalileoInav { + required uint32 gnssWN =1; + required uint32 gnssTOW =2; // INTEGERS! + + required uint32 gnssID =3; + required uint32 gnssSV =4; + required bytes contents =5; + } + + message ReceptionData { + required uint32 gnssID =1; + required uint32 gnssSV =2; + required uint32 db =3; + required uint32 el =4; + required uint32 azi =5; + required double prRes =6; + } + + message RFData { + required uint32 gnssID =1; + required uint32 gnssSV =2; + required double doppler =3; + required double carrierphase =4; + } + + message ObserverPosition { + required double x = 1; + required double y = 2; + required double z = 3; + required double accCm = 4; + } + + optional GalileoInav gi=5; + optional ReceptionData rd=6; + optional RFData rfd=7; + optional ObserverPosition op=8; +} diff --git a/navparse.cc b/navparse.cc new file mode 100644 index 0000000..983d635 --- /dev/null +++ b/navparse.cc @@ -0,0 +1,902 @@ +#include +#include +#include +#include +#include "fmt/format.h" +#include "fmt/printf.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include "ext/powerblog/h2o-pp.hh" +#include "minicurl.hh" +#include +#include "ubx.hh" +#include "bits.hh" +#include "minivec.hh" +#include "navmon.pb.h" + +using namespace std; + +struct EofException{}; + +Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000); + +int g_dtLS{18}; + + +string humanSisa(uint8_t sisa) +{ + unsigned int sval = sisa; + if(sisa < 50) + return std::to_string(sval)+" cm"; + if(sisa < 75) + return std::to_string(50 + 2* (sval-50))+" cm"; + if(sisa < 100) + return std::to_string(100 + 4*(sval-75))+" cm"; + if(sisa < 125) + return std::to_string(200 + 16*(sval-100))+" cm"; + if(sisa < 255) + return "SPARE"; + return "NO SIS AVAILABLE"; +} + + + +struct SVIOD +{ + std::bitset<32> words; + 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}; + uint16_t t0c; // clock epoch + int32_t af0, af1; + int8_t af2; + + uint8_t sisa; + + uint32_t wn{0}, tow{0}; + bool complete() const + { + return words[1] && words[2] && words[3] && words[4]; + } + 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 = getbitu(&page[0], 16, 14); + m0 = getbits(&page[0], 30, 32); + e = getbitu(&page[0], 62, 32); + sqrtA = getbitu(&page[0], 94, 32); + } + 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< iods; + void addWord(std::basic_string_view page); + bool completeIOD() const; + uint16_t getIOD() const; + SVIOD liveIOD() const; + + pair prevIOD{-1, SVIOD()}; + void clearPrev() + { + prevIOD.first = -1; + } +}; + +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 == 0) { + if(getbitu(&page[0], 6,2) == 2) { + wn = getbitu(&page[0], 96, 12); + tow = getbitu(&page[0], 108, 20); + } + } + else if(wtype >=1 && wtype <= 4) { // ephemeris + uint16_t iod = getbitu(&page[0], 6, 10); + iods[iod].addWord(page); + + if(iods[iod].complete()) { + for(const auto& i : iods) { + if(i.first != iod && i.second.complete()) + prevIOD=i; + } + SVIOD latest = iods[iod]; + iods.clear(); + iods[iod] = latest; + } + } + 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); + tow = getbitu(&page[0], 85, 20); + } + else if(wtype == 6) { + a0 = getbits(&page[0], 6, 32); + a1 = getbits(&page[0], 38, 24); + dtLS = getbits(&page[0], 62, 8); + + t0t = getbitu(&page[0], 70, 8); + wn0t = getbitu(&page[0], 78, 8); + wnLSF = getbitu(&page[0], 86, 8); + dn = getbitu(&page[0], 94, 3); + dtLSF = getbits(&page[0], 97, 8); + + // cout<<(int) dtLS << " " <<(int) wnLSF<<" " <<(int) dn <<" " <<(int) dtLSF<x = xprime * cos(Omega) - yprime * cos(i) * sin(Omega); + p->y = xprime * sin(Omega) + yprime * cos(i) * cos(Omega); + p->z = yprime * sin(i); + + if(!quiet) { + Point core(0.0, .0, .0); + Vector radius(core, *p); + cout << radius.length() << " calculated r "< g_svstats; + +int latestWN() +{ + map ages; + for(const auto& s: g_svstats) + 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"); + return g_svstats[ages.rbegin()->second].wn; +} + +int latestTow() +{ + map ages; + for(const auto& s: g_svstats) + 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"); + return g_svstats[ages.rbegin()->second].tow; +} + + + +uint64_t nanoTime(int wn, int tow) +{ + return 1000000000ULL*(935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!! +} + +uint64_t utcFromGST(int wn, int tow) +{ + return (935280000 + wn * 7*86400 + tow - g_dtLS); // Leap!! +} + +double utcFromGST(int wn, double tow) +{ + return (935280000.0 + wn * 7*86400 + tow - g_dtLS); // Leap!! +} + + +struct InfluxPusher +{ + explicit InfluxPusher(std::string_view dbname) : d_dbname(dbname) + { + } + void addValue( const pair& ent, string_view name, auto value) + { + d_buffer+= string(name) +",sv=" +to_string(ent.first)+" value="+to_string(value)+ + " "+to_string(nanoTime(ent.second.wn, ent.second.tow))+"\n"; + checkSend(); + } + + void addValue(int sv, string_view name, auto value) + { + d_buffer+= string(name) +",sv=" +to_string(sv) + " value="+to_string(value)+" "+ + to_string(nanoTime(g_svstats[sv].wn, g_svstats[sv].tow))+"\n"; + checkSend(); + } + + void checkSend() + { + if(d_buffer.size() > 1000000 || (time(0) - d_lastsent) > 10) { + string buffer; + buffer.swap(d_buffer); + // thread t([buffer,this]() { + doSend(buffer); + // }); + // t.detach(); + d_lastsent=time(0); + } + } + + void doSend(std::string buffer) + { + MiniCurl mc; + MiniCurl::MiniCurlHeaders mch; + if(!buffer.empty()) { + mc.postURL("http://127.0.0.1:8086/write?db="+d_dbname, buffer, mch); + } + } + + ~InfluxPusher() + { + doSend(d_buffer); + } + + std::string d_buffer; + time_t d_lastsent{0}; + string d_dbname; +}; + +/* The GST start epoch is defined as 13 seconds before midnight between 21st August and +22nd August 1999, i.e. GST was equal to 13 seconds at 22nd August 1999 00:00:00 UTC. */ + +std::string humanTime(int wn, int tow) +{ + time_t t = utcFromGST(wn, tow); + struct tm tm; + gmtime_r(&t, &tm); + + char buffer[80]; + strftime(buffer, sizeof(buffer), "%a, %d %b %Y %T %z", &tm); + return buffer; +} + +/* | t0e tow | - > tow - t0e, <3.5 days, so ok + + | t0e tow | -> tow - t0e > 3.5 days, so + 7*86400 - tow + t0e + + | tow t0e | -> 7*86400 - tow + t0e > 3.5 days, so + tow - t0e (negative age) + + | tow t0e | -> 7*86400 - tow + t0e < 3.5 days, ok +*/ + +int ephAge(int tow, int t0e) +{ + unsigned int diff; + unsigned int halfweek = 0.5*7*86400; + if(t0e < tow) { + diff = tow - t0e; + if(diff < halfweek) + return diff; + else + return (7*86400 - tow) + t0e; + } + else { // "t0e in future" + diff = 7*86400 - t0e + tow; + if(diff < halfweek) + return diff; + else + return tow - t0e; // in the future, negative age + } +} + +int main(int argc, char** argv) +try +{ + signal(SIGPIPE, SIG_IGN); + InfluxPusher idb(argc > 3 ? argv[3] : "galileo"); + MiniCurl::init(); + + H2OWebserver h2s("galmon"); + + h2s.addHandler("/global", [](auto handler, auto req) { + nlohmann::json ret = nlohmann::json::object(); + ret["leap-seconds"] = g_dtLS; + + map utcstats, gpsgststats; + for(const auto& s: g_svstats) { + int dw = (uint8_t)s.second.wn - s.second.wn0t; + int age = dw * 7 * 86400 + s.second.tow - s.second.t0t * 3600; + utcstats[age]=s.first; + + 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; + + } + if(utcstats.empty()) { + ret["utc-offset-ns"]=nullptr; + } + else { + int sv = utcstats.begin()->second; // freshest SV + long shift = g_svstats[sv].a0 * (1LL<<20) + g_svstats[sv].a1 * utcstats.begin()->first; // in 2^-50 seconds units + ret["utc-offset-ns"] = 1.073741824*ldexp(shift, -20); + + ret["leap-second-planned"] = (g_svstats[sv].dtLSF != g_svstats[sv].dtLS); + } + + if(gpsgststats.empty()) { + ret["gps-offset-ns"]=nullptr; + } + else { + int sv = gpsgststats.begin()->second; // freshest SV + long shift = g_svstats[sv].a0g * (1U<<16) + g_svstats[sv].a1g * utcstats.begin()->first; // in 2^-51 seconds units + + ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21); + } + + + return ret; + }); + + h2s.addHandler("/svs", [](auto handler, auto req) { + nlohmann::json ret = nlohmann::json::object(); + for(const auto& s: g_svstats) + if(s.second.completeIOD()) { + nlohmann::json item = nlohmann::json::object(); + + item["iod"]=s.second.getIOD(); + item["sisa"]=humanSisa(s.second.liveIOD().sisa); + item["a0"]=s.second.a0; + item["a1"]=s.second.a1; + item["dtLS"]=s.second.dtLS; + item["a0g"]=s.second.a0g; + item["a1g"]=s.second.a1g; + item["e5bdvs"]=s.second.e5bdvs; + item["e1bdvs"]=s.second.e1bdvs; + item["e5bhs"]=s.second.e5bhs; + item["e1bhs"]=s.second.e1bhs; + item["elev"]=s.second.el; + item["db"]=s.second.db; + item["eph-age-m"] = ephAge(s.second.tow, 60*s.second.liveIOD().t0e)/60.0; + item["last-seen-s"] = s.second.tow ? (7*86400*(s.second.wn - s.second.wn) + latestTow() - (int)s.second.tow) : -1; + + item["af0"] = s.second.liveIOD().af0; + item["af1"] = s.second.liveIOD().af1; + item["af2"] = (int)s.second.liveIOD().af2; + item["t0c"] = s.second.liveIOD().t0c; + + Point our = g_ourpos; + Point p; + Point core; + + // this should actually use local time! + getCoordinates(latestWN(), latestTow(), s.second.liveIOD(), &p); + + Vector core2us(core, our); + Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz; + + /* https://gis.stackexchange.com/questions/58923/calculating-view-angle + to calculate elevation: + Cos(elevation) = (x*dx + y*dy + z*dz) / Sqrt((x^2+y^2+z^2)*(dx^2+dy^2+dz^2)) + Obtain its principal inverse cosine. Subtract this from 90 degrees if you want + the angle of view relative to a nominal horizon. This is the "elevation." + NOTE! x = you on the ground! + */ + double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length())); + double deg = 180.0* (elev/M_PI); + item["calc-elev"] = 90 - deg; + + cout< (" + << p.x << ", "<< p.y <<", "<< p.z<<") "< 2 ? argv[2] : "./html/"); + + int port = argc > 1 ? atoi(argv[1]) : 29599; + std::thread ws([&h2s, port]() { + auto actx = h2s.addContext(); + h2s.addListener(ComboAddress("::", port), actx); + cout<<"Listening on port "<< port < inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size()); + int sv = nmm.gi().gnsssv(); + g_svstats[sv].wn = nmm.gi().gnsswn(); + g_svstats[sv].tow = nmm.gi().gnsstow(); + + // cout<<"inav for "<=1 && wtype <= 4) { // ephemeris + uint16_t iod = getbitu(&inav[0], 6, 10); + if(wtype == 3) { + idb.addValue(sv, "sisa", g_svstats[sv].iods[iod].sisa); + } + else if(wtype == 4) { + + idb.addValue(sv, "af0", g_svstats[sv].iods[iod].af0); + idb.addValue(sv, "af1", g_svstats[sv].iods[iod].af1); + idb.addValue(sv, "af2", g_svstats[sv].iods[iod].af2); + idb.addValue(sv, "t0c", g_svstats[sv].iods[iod].t0c); + + double age = ephAge(g_svstats[sv].tow, g_svstats[sv].iods[iod].t0c * 60); + + double offset = ldexp(1000.0*(1.0*g_svstats[sv].iods[iod].af0 + ldexp(age*g_svstats[sv].iods[iod].af1, -12)), -34); + idb.addValue(sv, "atomic_offset_ns", 1000000.0*offset); + } + else + ; + } + else if(wtype == 5) { + idb.addValue(sv, "ai0", g_svstats[sv].ai0); + idb.addValue(sv, "ai1", g_svstats[sv].ai1); + idb.addValue(sv, "ai2", g_svstats[sv].ai2); + + idb.addValue(sv, "sf1", g_svstats[sv].sf1); + idb.addValue(sv, "sf2", g_svstats[sv].sf2); + idb.addValue(sv, "sf3", g_svstats[sv].sf3); + idb.addValue(sv, "sf4", g_svstats[sv].sf4); + idb.addValue(sv, "sf5", g_svstats[sv].sf5); + + idb.addValue(sv, "BGDE1E5a", g_svstats[sv].BGDE1E5a); + idb.addValue(sv, "BGDE1E5b", g_svstats[sv].BGDE1E5b); + + idb.addValue(sv, "e1bhs", g_svstats[sv].e1bhs); + idb.addValue(sv, "e5bhs", g_svstats[sv].e5bhs); + idb.addValue(sv, "e5bdvs", g_svstats[sv].e5bdvs); + idb.addValue(sv, "e1bdvs", g_svstats[sv].e1bdvs); + } + else if(wtype == 6) { // GST-UTC + idb.addValue(sv, "a0", g_svstats[sv].a0); + idb.addValue(sv, "a1", g_svstats[sv].a1); + + g_dtLS = g_svstats[sv].dtLS; + } + else if(wtype == 10) { // GSTT GPS + idb.addValue(sv, "a0g", g_svstats[sv].a0g); + idb.addValue(sv, "a1g", g_svstats[sv].a1g); + idb.addValue(sv, "t0g", g_svstats[sv].t0g); + } + + for(auto& ent : g_svstats) { + // fmt::printf("%2d\t", ent.first); + if(ent.second.completeIOD() && ent.second.prevIOD.first >= 0) { + time_t t = utcFromGST((int)ent.second.wn, (int)ent.second.tow); + sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl; + // cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n"; + + double clockage = ephAge(ent.second.tow, ent.second.liveIOD().t0c * 60); + double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46); + clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + ent.second.wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset< str) @@ -58,3 +59,31 @@ std::basic_string buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, st */ return msg; } + +basic_string getInavFromSFRBXMsg(std::basic_string_view msg) +{ + // byte order adjustment + std::basic_string 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]); + + /* test crc (4(pad) + 114 + 82 bits) */ + unsigned char crc_buff[26]={0}; + unsigned int i,j; + for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8)); + for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8)); + if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) { + cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) < inav; + + 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)); + + return inav; +} diff --git a/ubx.hh b/ubx.hh index d163b36..373e0c4 100644 --- a/ubx.hh +++ b/ubx.hh @@ -4,3 +4,7 @@ uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_vi std::basic_string buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view str); std::basic_string buildUbxMessage(uint8_t ubxClass, uint8_t ubxType, const std::initializer_list& str); + +std::basic_string getInavFromSFRBXMsg(std::basic_string_view msg); + +struct CRCMismatch{}; diff --git a/ubxparse.cc b/ubxparse.cc index ee1068e..9fa14cb 100644 --- a/ubxparse.cc +++ b/ubxparse.cc @@ -47,20 +47,24 @@ Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000); int g_tow, g_wn, g_dtLS{18}; /* inav schedule: +1) Find plausible start time of next cycle + Current cycle: TOW - (TOW%30) + Next cycle: TOW - (TOW%30) + 30 + t n w -0 1: 2 wn % 30 == 0 -2 2: 4 wn % 30 == 2 -4 3: 6 -6 4: 7/9 -8 5: 8/10 -10 6: 0 WN/TOW +0 1: 2 wn % 30 == 0 +2 2: 4 wn % 30 == 2 +4 3: 6 WN/TOW 4 -> set startTow, startTowFresh +6 4: 7/9 +8 5: 8/10 +10 6: 0 TOW 12 7: 0 WN/TOW 14 8: 0 WN/TOW 16 9: 0 WN/TOW 18 10: 0 WN/TOW -20 11: 1 +20 11: 1 22 12: 3 -24 13: 5 +24 13: 5 WN/TOW 26 14: 0 WN/TOW 28 15: 0 WN/TOW */ @@ -550,14 +554,22 @@ try nlohmann::json ret = nlohmann::json::object(); ret["leap-seconds"] = g_dtLS; - map utcstats; + map utcstats, gpsgststats; for(const auto& s: g_svstats) { + // XXX this might run BEFORE we have gps/utc stats! + int dw = (uint8_t)g_wn - s.second.wn0t; int age = dw * 7 * 86400 + g_tow - s.second.t0t * 3600; utcstats[age]=s.first; + + uint8_t wn0g = s.second.wn0t; + int dwg = (((uint8_t)g_wn)&(1+2+4+8+16+32)) - wn0g; + age = dwg*7*86400 + g_tow - s.second.t0g * 3600; + gpsgststats[age]=s.first; + } if(utcstats.empty()) { - ret["utc-offset"]=nullptr; + ret["utc-offset-ns"]=nullptr; } else { int sv = utcstats.begin()->second; // freshest SV @@ -566,6 +578,18 @@ try ret["leap-second-planned"] = (g_svstats[sv].dtLSF != g_svstats[sv].dtLS); } + + if(gpsgststats.empty()) { + ret["gps-offset-ns"]=nullptr; + } + else { + int sv = gpsgststats.begin()->second; // freshest SV + long shift = g_svstats[sv].a0g * (1U<<16) + g_svstats[sv].a1g * utcstats.begin()->first; // in 2^-51 seconds units + + ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21); + } + + return ret; }); @@ -657,7 +681,7 @@ try clockcsv <<"timestamp sv af0 af1 af2 t0c age offset"< 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]); + try { + unsigned int sv = (int)msg[1]; + basic_string inav = getInavFromSFRBXMsg(msg); + + // cout<<"inav for "<=1 && wtype <= 4) { // ephemeris + uint16_t iod = getbitu(&inav[0], 6, 10); + if(wtype == 1 && g_tow) { + } + else if(wtype == 3) { - - /* test crc (4(pad) + 114 + 82 bits) */ - unsigned char crc_buff[26]={0}; - unsigned int i,j; - for (i=0,j= 4;i<15;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str() ,i*8,8)); - for (i=0,j=118;i<11;i++,j+=8) setbitu(crc_buff,j,8,getbitu(payload.c_str()+16,i*8,8)); - if (rtk_crc24q(crc_buff,25) != getbitu(payload.c_str()+16,82,24)) { - cout << "CRC mismatch, " << rtk_crc24q(crc_buff, 25) << " != " << getbitu(payload.c_str()+16,82,24) < inav; - - 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 && g_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); - } - else if(wtype == 3) { - // unsigned int sisa = getbitu(&inav[0], 120, 8); - idb.addValue(sv, "sisa", g_svstats[sv].iods[iod].sisa); - } - else if(wtype == 4) { - - idb.addValue(sv, "af0", g_svstats[sv].iods[iod].af0); - idb.addValue(sv, "af1", g_svstats[sv].iods[iod].af1); - idb.addValue(sv, "af2", g_svstats[sv].iods[iod].af2); - idb.addValue(sv, "t0c", g_svstats[sv].iods[iod].t0c); - - double age = ephAge(g_tow, g_svstats[sv].iods[iod].t0c * 60); - /* - cout<<"Atomic age "<= 0) { - time_t t = utcFromGST(g_wn, g_tow); - sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl; - // cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n"; - - double clockage = ephAge(g_tow, ent.second.liveIOD().t0c * 60); - double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46); - clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + g_wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset<= 0) { + time_t t = utcFromGST(g_wn, g_tow); + sisacsv << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << endl; + // cout << t <<" " << ent.first << " " << (unsigned int) ent.second.liveIOD().sisa << "\n"; + + double clockage = ephAge(g_tow, ent.second.liveIOD().t0c * 60); + double offset = 1.0*ent.second.liveIOD().af0/(1LL<<34) + clockage * ent.second.liveIOD().af1/(1LL<<46); + clockcsv << t << " " << ent.first<<" " << ent.second.liveIOD().af0 << " " << ent.second.liveIOD().af1 <<" " << (int)ent.second.liveIOD().af2 <<" " << 935280000 + g_wn *7*86400 + ent.second.liveIOD().t0c * 60 << " " << clockage << " " << offset< #include "fmt/format.h" #include "fmt/printf.h" +#include "bits.hh" +#include "galileo.hh" +#include +#include "navmon.pb.h" +struct timespec g_gstutc; +uint16_t g_wn; using namespace std; #define BAUDRATE B921600 @@ -67,10 +73,57 @@ size_t writen2(int fd, const void *buf, size_t count) return count; } +/* inav schedule: +1) Find plausible start time of next cycle + Current cycle: TOW - (TOW%30) + Next cycle: TOW - (TOW%30) + 30 + +t n w +0 1: 2 wn % 30 == 0 +2 2: 4 wn % 30 == 2 +4 3: 6 WN/TOW 4 -> set startTow, startTowFresh +6 4: 7/9 +8 5: 8/10 +10 6: 0 TOW +12 7: 0 WN/TOW +14 8: 0 WN/TOW +16 9: 0 WN/TOW +18 10: 0 WN/TOW +20 11: 1 +22 12: 3 +24 13: 5 WN/TOW +26 14: 0 WN/TOW +28 15: 0 WN/TOW +*/ + +/* + if(ubxClass == 2 && ubxType == 89) { // SAR + string hexstring; + for(int n = 0; n < 15; ++n) + hexstring+=fmt::format("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4)); + + // int sv = (int)msg[2]; + // wk.emitLine(sv, "SAR "+hexstring); + // cout<<"SAR: sv = "<< (int)msg[2] <<" "; + // for(int n=4; n < 12; ++n) + // fmt::printf("%02x", (int)msg[n]); + + // for(int n = 0; n < 15; ++n) + // fmt::printf("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4)); + + // cout << " Type: "<< (int) msg[12] <<"\n"; + // cout<<"Parameter: (len = "< src) { d_raw = src; @@ -79,7 +132,7 @@ public: uint16_t csum = calcUbxChecksum(getClass(), getType(), d_raw.substr(6, d_raw.size()-8)); if(csum != d_raw.at(d_raw.size()-2) + 256*d_raw.at(d_raw.size()-1)) - throw std::runtime_error("Bad UBX checksum"); + throw BadChecksum(); } uint8_t getClass() const @@ -97,7 +150,7 @@ public: std::basic_string d_raw; }; -std::pair getUBXMessage(int fd) +std::pair getUBXMessage(int fd) { uint8_t marker[2]={0}; for(;;) { @@ -123,7 +176,7 @@ std::pair getUBXMessage(int fd) res=readn2(fd, buffer, len+2); msg.append(buffer, len+2); // checksum - return make_pair(tv, UBXMessage(msg)); + return make_pair(UBXMessage(msg), tv); } } } @@ -131,7 +184,8 @@ std::pair getUBXMessage(int fd) UBXMessage waitForUBX(int fd, int seconds, uint8_t ubxClass, uint8_t ubxType) { for(int n=0; n < seconds*20; ++n) { - auto [timestamp, msg] = getUBXMessage(fd); + auto [msg, tv] = getUBXMessage(fd); + (void) tv; // cerr<<"Got: "<<(int)msg.getClass() << " " <<(int)msg.getType() < 1) { + fromFile = true; + fd = open(argv[1], O_RDONLY ); } - - std::basic_string msg; - if(argc> 1 && string(argv[1])=="cold") { - cerr<<"Sending cold start!"< msg; + cerr<<"Asking for rate"<, struct timeval> lasttv, tv; - + unsigned int nextCycleTOW{0}; + unsigned int curCycleTOW{0}; + bool curCycleTOWFresh=false; for(;;) { - auto [timestamp, msg] = getUBXMessage(fd); - auto payload = msg.getPayload(); + try { + auto [msg, timestamp] = getUBXMessage(fd); + (void)timestamp; + auto payload = msg.getPayload(); + // should turn this into protobuf + if(msg.getClass() == 0x01 && msg.getType() == 0x07) { // UBX-NAV-PVT + struct PVT + { + uint32_t itow; + uint16_t year; + uint8_t month; // jan = 1 + uint8_t day; + uint8_t hour; // 24 + uint8_t min; + uint8_t sec; + uint8_t valid; + uint32_t tAcc; + int32_t nano; + uint8_t fixtype; + } __attribute__((packed)); + PVT pvt; + + memcpy(&pvt, &payload[0], sizeof(pvt)); + struct tm tm; + memset(&tm, 0, sizeof(tm)); + tm.tm_year = pvt.year - 1900; + tm.tm_mon = pvt.month - 1; + tm.tm_mday = pvt.day; + tm.tm_hour = pvt.hour; + tm.tm_min = pvt.min; + tm.tm_sec = pvt.sec; - if(msg.getClass() == 0x01 && msg.getType() == 0x01) { // POSECF - struct pos - { - uint32_t iTOW; - int32_t ecefX; - int32_t ecefY; - int32_t ecefZ; - uint32_t pAcc; - }; - pos p; - memcpy(&p, payload.c_str(), sizeof(pos)); - cerr<<"Position: ("<< p.ecefX / 100000.0<<", " - << p.ecefY / 100000.0<<", " - << p.ecefZ / 100000.0<<") +- "< id = make_pair(payload[0], payload[1]); - tv[id] = timestamp; - if(lasttv.count(id)) { - fmt::fprintf(stderr, "gnssid %d sv %d, %d:%d -> %d:%d, delta=%d\n", - payload[0], payload[1], lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec); + uint32_t satt = timegm(&tm); + double satutc = timegm(&tm) + pvt.nano/1000000000.0; // negative is no problem here + if(pvt.nano < 0) { + pvt.sec--; + satt--; + pvt.nano += 1000000000; + } + + g_gstutc.tv_sec = satt; + g_gstutc.tv_nsec = pvt.nano; + + + double seconds= pvt.sec + pvt.nano/1000000000.0; + fmt::fprintf(stderr, "Satellite UTC: %02d:%02d:%06.4f -> %.4f or %d:%f\n", tm.tm_hour, tm.tm_min, seconds, satutc, timegm(&tm), pvt.nano/1000.0); + + if(!fromFile) { + struct tm ourtime; + time_t ourt = timestamp.tv_sec; + gmtime_r(&ourt, &ourtime); + + double ourutc = ourt + timestamp.tv_usec/1000000.0; + + seconds = ourtime.tm_sec + timestamp.tv_usec/1000000.0; + fmt::fprintf(stderr, "Our UTC : %02d:%02d:%06.4f -> %.4f or %d:%f -> delta = %.4fs\n", tm.tm_hour, tm.tm_min, seconds, ourutc, timestamp.tv_sec, 1.0*timestamp.tv_usec, ourutc - satutc); + } } - lasttv[id]=tv[id]; - } - writen2(1, msg.d_raw.c_str(),msg.d_raw.size()); -#if 0 - if(msg.getClass() == 0x02 && msg.getType() == 0x15) { // RAWX - cerr<<"Got "<<(int)payload[11] <<" measurements "<set_x(p.ecefX /100.0); + nmm.mutable_op()->set_y(p.ecefY /100.0); + nmm.mutable_op()->set_z(p.ecefZ /100.0); + nmm.mutable_op()->set_accCM(p.pAcc /100.0); + emitNMM(1, nmm); } - } -#endif + else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX + pair id = make_pair(payload[0], payload[1]); + auto inav = getInavFromSFRBXMsg(payload); + unsigned int wtype = getbitu(&inav[0], 0, 6); + tv[id] = timestamp; + cerr<<"gnssid "<set_gnsswn(g_wn); + nmm.mutable_gi()->set_gnsstow(msgTOW); + nmm.mutable_gi()->set_gnssid(id.first); + nmm.mutable_gi()->set_gnsssv(id.second); + nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size()); + + emitNMM(1, nmm); + } + + + if(0 && lasttv.count(id)) { + fmt::fprintf(stderr, "gnssid %d sv %d wtype %d, %d:%d -> %d:%d, delta=%d\n", + payload[0], payload[1], wtype, lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec); + } + lasttv[id]=tv[id]; + } + else if(msg.getClass() == 1 && msg.getType() == 0x35) { // UBX-NAV-SAT + cerr<< "Info for "<<(int) payload[5]<<" svs: \n"; + for(unsigned int n = 0 ; n < payload[5]; ++n) { + cerr << " "<<(payload[8+12*n] ? 'E' : 'G') << (int)payload[9+12*n] <<" db="; + cerr << (int)payload[10+12*n]<<" elev="<<(int)(char)payload[11+12*n]<<" azi="; + cerr << ((int)payload[13+12*n]*256 + payload[12+12*n])<<" prres="<< *((int16_t*)(payload.c_str()+ 14 +12*n)) *0.1 << " signal="<< ((int)(payload[16+12*n])&7) << " used="<< (payload[16+12*n]&8); + + fmt::fprintf(stderr, " | %02x %02x %02x %02x\n", (int)payload[16+12*n], (int)payload[17+12*n], + (int)payload[18+12*n], (int)payload[19+12*n]); + + if(payload[8+12*n]==2) { // galileo + int sv = payload[9+12*n]; + auto el = (int)(char)payload[11+12*n]; + auto azi = ((int)payload[13+12*n]*256 + payload[12+12*n]); + auto db = (int)payload[10+12*n]; + + NavMonMessage nmm; + nmm.set_sourceid(1); + nmm.set_localutcseconds(g_gstutc.tv_sec); + nmm.set_localutcnanoseconds(g_gstutc.tv_nsec); + + nmm.set_type(NavMonMessage::ReceptionDataType); + nmm.mutable_rd()->set_gnssid(payload[8+12*n]); + nmm.mutable_rd()->set_gnsssv(sv); + nmm.mutable_rd()->set_db(db); + nmm.mutable_rd()->set_el(el); + nmm.mutable_rd()->set_azi(azi); + nmm.mutable_rd()->set_prres(*((int16_t*)(payload.c_str()+ 14 +12*n)) *0.1); + emitNMM(1, nmm); + + + } + } + } + + // writen2(1, payload.d_raw.c_str(),msg.d_raw.size()); + } + catch(UBXMessage::BadChecksum &e) { + cerr<<"Bad UBX checksum, skipping message"<