diff --git a/Makefile b/Makefile index 2e76f6d..515a776 100644 --- a/Makefile +++ b/Makefile @@ -22,7 +22,7 @@ clean: rm -f ext/fmt-5.2.1/src/format.o -navparse: navparse.o ext/fmt-5.2.1/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 +navparse: navparse.o ext/fmt-5.2.1/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 $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY) navdump: navdump.o ext/fmt-5.2.1/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 diff --git a/README.md b/README.md index 5628221..a0fe37e 100644 --- a/README.md +++ b/README.md @@ -79,7 +79,7 @@ Running ------- Once compiled, run for example `./ubxtool --wait --port /dev/ttyACM0 ---station 1 --stdout | ./ubxparse 10000 html null` +--station 1 --stdout | ./navparse 10000 html null` Next up, browse to http://[::1]:10000 (or try http://localhost:10000/ and you should be in business. ubxtool changes (non-permanently) the diff --git a/coverage.cc b/coverage.cc new file mode 100644 index 0000000..181dfd4 --- /dev/null +++ b/coverage.cc @@ -0,0 +1,78 @@ +#include +#include "galileo.hh" +#include "minivec.hh" +#include "navmon.hh" +#include "navparse.hh" +#include "ephemeris.hh" +#include "fmt/format.h" +#include "fmt/printf.h" +#include + +using namespace std; +extern GetterSetter> g_galileoalmakeeper; +extern GetterSetter g_statskeeper; + + +covmap_t emitCoverage() +{ + covmap_t ret; + ofstream cmap("covmap.csv"); + cmap<<"latitude longitude count5 count10 count20"< sats; + auto galileoalma = g_galileoalmakeeper.get(); + auto svstats = g_statskeeper.get(); + auto pseudoTow = (time(0) - 820368000) % (7*86400); + // cout<<"pseudoTow "< -90; latitude-=0.5) { // north-south + double phi = M_PI* latitude / 180; + double longsteps = 1 + 360.0 * cos(phi); + double step = 180.0 / longsteps; + vector> latvect; + for(double longitude = -180; longitude < 180; longitude += step) { // east - west + Point p; + // phi = latitude, lambda = longitude + + double lambda = M_PI* longitude / 180; + p.x = R * cos(phi) * cos(lambda); + p.y = R * cos(phi) * sin(lambda); + p.z = R * sin(phi); + + if(longitude == -180) { + auto longlat = getLongLat(p.x, p.y, p.z); + // cout< 5.0) + numsats5++; + if(elev > 10.0) + numsats10++; + if(elev > 20.0) + numsats20++; + } + if(numsats20 < 4) + latvect.push_back(make_tuple(longitude, numsats5, numsats10, numsats20)); + // cmap << longitude <<" " <= parsers.size()) { - std::cerr<<"Asked for impossible galileo type "< #include #include +#include struct EofException{}; size_t readn2(int fd, void* buffer, size_t len); @@ -20,3 +21,28 @@ struct SatID return std::tie(gnss, sv, sigid) < std::tie(rhs.gnss, rhs.sv, rhs.sigid); } }; + +template +class GetterSetter +{ +public: + void set(const T& t) + { + std::lock_guard mut(d_lock); + d_t = t; + } + + T get() + { + T ret; + { + std::lock_guard mut(d_lock); + ret = d_t; + } + return ret; + } +private: + T d_t; + std::mutex d_lock; +}; + diff --git a/navparse.cc b/navparse.cc index 1963e4c..b6154b9 100644 --- a/navparse.cc +++ b/navparse.cc @@ -27,6 +27,7 @@ #include #include "navmon.hh" #include +#include "navparse.hh" using namespace std; struct ObserverPosition @@ -36,31 +37,6 @@ struct ObserverPosition }; std::map g_srcpos; -template -class GetterSetter -{ -public: - void set(const T& t) - { - std::lock_guard mut(d_lock); - d_t = t; - } - - T get() - { - T ret; - { - std::lock_guard mut(d_lock); - ret = d_t; - } - return ret; - } -private: - T d_t; - std::mutex d_lock; -}; - - map g_beidoualma; map> g_glonassalma; @@ -117,7 +93,7 @@ string humanSisa(uint8_t sisa) return std::to_string(200 + 16*(sval-100))+" cm"; if(sisa < 255) return "SPARE"; - return "NO SIS AVAILABLE"; + return "NO SISA AVAILABLE"; } string humanUra(uint8_t ura) @@ -130,64 +106,6 @@ string humanUra(uint8_t ura) } -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 - - -}; void SVIOD::addGalileoWord(std::basic_string_view page) { @@ -231,78 +149,7 @@ void SVIOD::addGalileoWord(std::basic_string_view page) } -struct SVPerRecv -{ - int el{-1}, azi{-1}, db{-1}; - time_t deltaHzTime{-1}; - double deltaHz{-1}; - double prres{-1}; - time_t t; // last seen -}; - -/* Most of thes fields are raw, EXCEPT: - t0t = seconds, raw fields are 3600 seconds (galileo), 4096 seconds (GPS) -*/ - -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) - - GPSAlmanac gpsalma; - - // beidou: - int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1}; - BeidouMessage beidouMessage, oldBeidouMessage; - BeidouMessage lastBeidouMessage1, lastBeidouMessage2; - TLERepo::Match tleMatch; - double lastTLELookupX{0}; - - // new galileo - GalileoMessage galmsg; - map galmsgTyped; - - // Glonass - GlonassMessage glonassMessage; - 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); -}; bool SVStat::completeIOD() const { @@ -409,7 +256,6 @@ void SVStat::addGalileoWord(std::basic_string_view page) } -typedef std::map svstats_t; svstats_t g_svstats; GetterSetter g_statskeeper; @@ -657,7 +503,7 @@ std::optional getHzCorrection(time_t now, int src, unsigned int gnssid, std::string humanBhs(int bhs) { static vector options{"ok", "out of service", "will be out of service", "test"}; - if(bhs >= options.size()) { + if(bhs >= (int)options.size()) { cerr<<"Asked for humanBHS "<second.wn, iter->second.tow)/1000000000 < 300) + + if(iter->second.completeIOD()) { + item["sisa"] = iter->second.liveIOD().sisa; + } + // if we hit an 'observed', stop trying sigids + if(time(0) - nanoTime(2, iter->second.wn, iter->second.tow)/1000000000 < 300) { item["observed"] = true; + break; + } + + } } @@ -1027,7 +882,6 @@ try h2s.addHandler("/sv.json", [](auto handler, auto req) { string_view path = convert(req->path); - cout<(longpair)); + jsdatum.push_back(get<1>(longpair)); + jsdatum.push_back(get<2>(longpair)); + jsdatum.push_back(get<3>(longpair)); + jslongvect.push_back(jsdatum); + } + jslatvect.push_back(latvect.first); + jslatvect.push_back(jslongvect); + + ret.push_back(jslatvect); + } + return ret; + }); h2s.addHandler("/svs.json", [](auto handler, auto req) { auto svstats = g_statskeeper.get(); diff --git a/navparse.hh b/navparse.hh new file mode 100644 index 0000000..e616009 --- /dev/null +++ b/navparse.hh @@ -0,0 +1,149 @@ +#include "navmon.hh" +#include "galileo.hh" +#include "gps.hh" +#include "beidou.hh" +#include "glonass.hh" +#include +#include "tle.hh" +using namespace std; // XXX + +struct SVPerRecv +{ + int el{-1}, azi{-1}, db{-1}; + time_t deltaHzTime{-1}; + double deltaHz{-1}; + double prres{-1}; + 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) +*/ + +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) + + GPSAlmanac gpsalma; + + // beidou: + int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1}; + BeidouMessage beidouMessage, oldBeidouMessage; + BeidouMessage lastBeidouMessage1, lastBeidouMessage2; + TLERepo::Match tleMatch; + double lastTLELookupX{0}; + + // new galileo + GalileoMessage galmsg; + map galmsgTyped; + + // Glonass + GlonassMessage glonassMessage; + 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); +}; + + +typedef std::map svstats_t; + +// a vector of pairs of latidude,vector +typedef vector > > > covmap_t; + +covmap_t emitCoverage(); + diff --git a/tle.cc b/tle.cc index b5d61ea..fbacee2 100644 --- a/tle.cc +++ b/tle.cc @@ -98,7 +98,7 @@ TLERepo::Match TLERepo::getBestMatch(time_t now, double x, double y, double z, T distances.insert({1000.0*(rot - sat).Magnitude(),sgp4.first}); } catch(SatelliteException& se) { - cerr<<"TLE error: "< -#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" - -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; -} - -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 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 -*/ - - -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) { - g_wn = wn = getbitu(&page[0], 96, 12); - g_tow = 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); - g_wn = wn = getbitu(&page[0], 73, 12); - g_tow = 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; - -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 + 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(); - } - - // note: this version trusts the system-wide g_wn, g_tow ovre the local one - 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_wn, g_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) { - // 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-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(g_tow, 60*s.second.liveIOD().t0e)/60.0; - item["last-seen-s"] = s.second.tow ? (7*86400*(g_wn - s.second.wn) + (int)g_tow - (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; - - getCoordinates(g_wn, g_tow, 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 < 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()); - - if(sv < 37) { - g_svstats[sv].el = atoi(strs[n+1].c_str()); - g_svstats[sv].azi = atoi(strs[n+2].c_str()); - if(g_svstats[sv].el >= 0) - g_svstats[sv].db = atoi(strs[n+3].c_str()); - else - g_svstats[sv].db = -1; - - idb.addValue(sv, "db", g_svstats[sv].db); - idb.addValue(sv, "elev", g_svstats[sv].el); - idb.addValue(sv, "azi", g_svstats[sv].azi); - } - } - } - line.clear(); - } - continue; - } - c = getUint8(); - if(c != 0x62) { - ungetc(c, stdin); // might be 0xb5 - continue; - } - if(lastDisplay != time(0)) { - lastDisplay = time(0); - if(g_wn && g_tow) { - cout<<"UTC from satellite: "< 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< 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) { - - 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) - g_svstats[sv].db = (int)msg[10+12*n]; - else - g_svstats[sv].db = -1; - - idb.addValue(sv, "db", g_svstats[sv].db); - idb.addValue(sv, "elev", g_svstats[sv].el); - idb.addValue(sv, "azi", g_svstats[sv].azi); - } - } - } - else if (ubxClass == 4) { // info - fmt::printf("Log level %d: %.*s\n", (int)ubxType, msg.size(), (char*)msg.c_str()); - } - else if (ubxClass == 6 && ubxType == 0) { // port config - fmt::printf("Port config: "); - for(const auto& c : msg) - fmt::printf("0x%02x,", (int)c); - cout<<"\n"; - uint8_t outmask=msg[14]; - if(outmask & 1) - cout<<" out UBX"<