From 1a69de074ee43bb0a9a92746bd80db46b8a3daa7 Mon Sep 17 00:00:00 2001 From: bert hubert Date: Tue, 3 Sep 2019 20:05:40 +0200 Subject: [PATCH] interim --- Makefile | 6 +-- beidou.cc | 52 ++++++++++++++++++++++ beidou.hh | 27 ++++++++--- ephemeris.cc | 30 +++++-------- glonass.hh | 37 +++++++++++++-- navdisplay.cc | 7 +-- navdump.cc | 79 +++++++++++++++----------------- navnexus.cc | 2 +- navparse.cc | 121 +++++++++++++++++++++++++++++++++++++++++++------- testrunner.cc | 3 +- tle.cc | 12 +++-- tle.hh | 2 + 12 files changed, 279 insertions(+), 99 deletions(-) diff --git a/Makefile b/Makefile index 9d2e8ac..b39825b 100644 --- a/Makefile +++ b/Makefile @@ -17,13 +17,13 @@ clean: H2OPP=ext/powerblog/h2o-pp.o SIMPLESOCKETS=ext/powerblog/ext/simplesocket/swrappers.o ext/powerblog/ext/simplesocket/sclasses.o ext/powerblog/ext/simplesocket/comboaddress.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 +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 $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf # -lwslay 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 $(CXX) -std=gnu++17 $^ -o $@ -pthread -lprotobuf -navdisplay: navdisplay.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o +navdisplay: navdisplay.o ext/fmt-5.2.1/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o $(CXX) -std=gnu++17 $^ -o $@ -pthread -lprotobuf -lncurses @@ -39,7 +39,7 @@ tlecatch: tlecatch.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) navmon.pb.cc: navmon.proto protoc --cpp_out=./ navmon.proto -ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o navmon.o +ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o navmon.o ephemeris.o $(CXX) -std=gnu++17 $^ -o $@ -lprotobuf testrunner: navmon.pb.o testrunner.o ubx.o bits.o ext/fmt-5.2.1/src/format.o galileo.o gps.o beidou.o ephemeris.o diff --git a/beidou.cc b/beidou.cc index 8b2106e..bb896d2 100644 --- a/beidou.cc +++ b/beidou.cc @@ -1,6 +1,7 @@ #include "beidou.hh" #include "bits.hh" #include + using namespace std; // with immense gratitude to https://stackoverflow.com/questions/24612436/how-to-check-a-bch15-11-1-code-checksum-for-bds-beidou-satellite-system @@ -72,3 +73,54 @@ int beidouBitconv(int their) return their - (1 + 4 + (wordcount -1)*8); } + + +bool processBeidouAlmanac(const BeidouMessage& bm, struct BeidouAlmanacEntry& bae) +{ + bae.alma = bm.alma; + int pageno = bm.alma.pageno; + + if(bm.fraid == 5 && pageno >= 11 && pageno <= 23) { + /* + cout<<" AmEpID "<< bm.alma.AmEpID; + cout << " AmID "< cond) { - int pageno = bbitu(44, 7); - if(pageno == 9) { + alma.pageno = bbitu(44, 7); + if(alma.pageno == 9) { a0gps = bbits(97, 14); a1gps = bbits(111, 16); a0gal = bbits(135, 14); @@ -233,7 +236,7 @@ struct BeidouMessage a0glo = bbits(181, 14); a1glo = bbits(195, 16); } - if(pageno == 10) { + if(alma.pageno == 10) { a0utc = bbits(91, 32); a1utc = bbits(131, 24); deltaTLS = bbits(31+12+1+7, 8); @@ -251,11 +254,21 @@ struct BeidouMessage alma.omega = bbits(227, 24); alma.M0 = bbits(259, 24); - alma.AmEpID = bbitu(291, 2); + if(alma.pageno <=6) + alma.AmEpID = bbitu(291, 2); + else if(alma.pageno >= 11 && alma.pageno <= 23) + alma.AmID = bbitu(291, 2); } - return pageno; + return alma.pageno; } }; +struct BeidouAlmanacEntry +{ + int sv; + BeidouMessage::Almanac alma; +}; + +bool processBeidouAlmanac(const BeidouMessage& bm, struct BeidouAlmanacEntry& bae); diff --git a/ephemeris.cc b/ephemeris.cc index e3ac820..b22487f 100644 --- a/ephemeris.cc +++ b/ephemeris.cc @@ -14,22 +14,12 @@ // positive age = t0e in the past 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 diff = tow - t0e; + if(diff > 3.5*86400) + diff -= 604800; + if(diff < -3.5*86400) + diff += 604800; + return diff; } // all axes start at earth center of gravity @@ -48,11 +38,13 @@ std::pair getLongLat(double x, double y, double z) Vector flat(core, proj); Vector toLatLon0{core, LatLon0}; double longitude = acos( toLatLon0.inner(flat) / (toLatLon0.length() * flat.length())); - - + if(y < 0) + longitude *= -1; + Vector toUs{core, pos}; double latitude = acos( flat.inner(toUs) / (toUs.length() * flat.length())); - + if(z < 0) + latitude *= -1; return std::make_pair(longitude, latitude); } diff --git a/glonass.hh b/glonass.hh index f05e123..32e7aae 100644 --- a/glonass.hh +++ b/glonass.hh @@ -131,17 +131,34 @@ struct GlonassMessage uint32_t getGloTime() const; uint8_t n4{0}; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in - uint16_t taugps; + int32_t taugps; + int32_t tauc; + void parse5(std::basic_string_view gstr) { n4=getbitu(&gstr[0], 85-36, 5); taugps = getbitsglonass(&gstr[0], 85-31, 22); + tauc = getbitsglonass(&gstr[0], 85-69, 32); l_n = getbitu(&gstr[0], 85 - 9, 1); } + double omegana; // 2^-15 semi-circles, at instantt of tlambdana + uint8_t hna; + uint32_t tlambdana; // 2^-5s time of ascending node passage, also: t0a, relative to MT midnight + int32_t deltatna; // 2^-9 + + double gettLambdaNa() const + { + return ldexp(tlambdana, -5); + } + void parse7_9_11_13_15(std::basic_string_view gstr) { l_n = getbitu(&gstr[0], 85 - 9, 1); + omegana = getbitsglonass(&gstr[0], 85-80, 16); + hna = getbitu(&gstr[0], 85 - 14, 5); // this is always positive, but there is a translation table + tlambdana = getbitu(&gstr[0], 85 - 64, 21); + deltatna = getbitsglonass(&gstr[0], 85 - 43, 22); } @@ -149,11 +166,23 @@ struct GlonassMessage bool CnA; int32_t lambdana; // 2^-20 semi-circles int32_t deltaina; // 2^-20 semi-circles - - double getLambdaNaDeg() + int32_t epsilonna; // 2^-20 + int32_t tauna; // 2^-18 + + double getLambdaNaDeg() const { return ldexp(180.0*lambdana, -20); } + + double getE() const + { + return ldexp(epsilonna, -20); + } + + double getI0() const + { + return M_PI*63.0/180 + ldexp(M_PI* deltaina, -20); + } void parse6_8_10_12_14(std::basic_string_view gstr) { @@ -161,6 +190,8 @@ struct GlonassMessage nA = getbitu(&gstr[0], 85-77, 5); lambdana = getbitsglonass(&gstr[0], 85-62, 21); deltaina = getbitsglonass(&gstr[0], 85-41, 18); + epsilonna = getbitu(&gstr[0], 85- 23, 15); + tauna = getbitsglonass(&gstr[0], 85 - 72, 10); } }; diff --git a/navdisplay.cc b/navdisplay.cc index 38159ac..4d73557 100644 --- a/navdisplay.cc +++ b/navdisplay.cc @@ -12,6 +12,7 @@ #include "galileo.hh" #include "navmon.pb.h" #include +#include "navmon.hh" using namespace std; @@ -142,17 +143,17 @@ int main() for(;;) { char bert[4]; - if(read(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') { + if(readn2(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') { cerr<<"EOF or bad magic"<=7 && wtype<=10) cout<<" ioda "< start = {0,0}; - start.first = time(0) - 4*3600; // 4 hours of backlog + start.first = time(0) - 24*3600; // 4 hours of backlog // so we have a ton of files, and internally these are not ordered map fpos; diff --git a/navparse.cc b/navparse.cc index 70e7d52..4ba6dd6 100644 --- a/navparse.cc +++ b/navparse.cc @@ -24,12 +24,15 @@ #include "galileo.hh" #include "tle.hh" #include +#include "navmon.hh" #include using namespace std; -struct EofException{}; Point g_ourpos(3922.505 * 1000, 290.116 * 1000, 5004.189 * 1000); +map g_beidoualma; +map> g_glonassalma; + TLERepo g_tles; struct GNSSReceiver { @@ -232,6 +235,7 @@ struct SVStat // Glonass GlonassMessage glonassMessage; + pair glonassAlmaEven; map perrecv; pair deltaHz; @@ -553,11 +557,13 @@ double getElevation(const Point& p) int main(int argc, char** argv) try { -// g_tles.parseFile("active.txt"); + // g_tles.parseFile("active.txt"); + g_tles.parseFile("galileo.txt"); g_tles.parseFile("glo-ops.txt"); g_tles.parseFile("gps-ops.txt"); g_tles.parseFile("beidou.txt"); + signal(SIGPIPE, SIG_IGN); InfluxPusher idb(argc > 3 ? argv[3] : "galileo"); MiniCurl::init(); @@ -633,6 +639,69 @@ try + return ret; + }); + + h2s.addHandler("/almanac", [](auto handler, auto req) { + nlohmann::json ret = nlohmann::json::object(); + for(const auto& ae : g_beidoualma) { + nlohmann::json item = nlohmann::json::object(); + if(ae.second.alma.getT0e() > 7*86400) + continue; + Point sat; + getCoordinates(0, latestTow(3), ae.second.alma, &sat); + item["eph-ecefX"]= sat.x/1000; + item["eph-ecefY"]= sat.y/1000; + item["eph-ecefZ"]= sat.z/1000; + + auto longlat = getLongLat(sat.x, sat.y, sat.z); + item["eph-longitude"] = 180*longlat.first/M_PI; + item["eph-latitude"]= 180*longlat.second/M_PI; + item["t0e"] = ae.second.alma.getT0e(); + item["t"]= ephAge(ae.second.alma.getT0e(), latestTow(3))/86400.0; + if(ephAge(ae.second.alma.getT0e(), latestTow(3)) < 0) { + auto match = g_tles.getBestMatch(nanoTime(3, latestWN(3), latestTow(3))/1000000000.0, + sat.x, sat.y, sat.z); + + if(match.distance < 200000) { + item["best-tle"] = match.name; + item["best-tle-norad"] = match.norad; + item["best-tle-int-desig"] = match.internat; + item["best-tle-dist"] = match.distance/1000.0; + + item["tle-ecefX"] = match.ecefX/1000; + item["tle-ecefY"] = match.ecefY/1000; + item["tle-ecefZ"] = match.ecefZ/1000; + + item["tle-eciX"] = match.eciX/1000; + item["tle-eciY"] = match.eciY/1000; + item["tle-eciZ"] = match.eciZ/1000; + + item["tle-latitude"] = 180*match.latitude/M_PI; + item["tle-longitude"] = 180*match.longitude/M_PI; + item["tle-altitude"] = match.altitude; + } + } + + ret[fmt::sprintf("C%02d", ae.first)] = item; + } + + for(const auto& ae : g_glonassalma) { + + nlohmann::json item = nlohmann::json::object(); + + // ae.second.first -> even ae.second.sceond -> odd + + item["e"] = ae.second.first.getE(); + item["inclination"] = 180 * ae.second.first.getI0() /M_PI; + item["health"] = ae.second.first.CnA; + item["tlambdana"] = ae.second.second.gettLambdaNa(); + item["lambdana"] = ae.second.second.getLambdaNaDeg(); + item["hna"] = ae.second.second.hna; + + ret[fmt::sprintf("R%02d", ae.first)] = item; + } + return ret; }); @@ -818,17 +887,17 @@ try try { for(;;) { char bert[4]; - if(read(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') { + if(readn2(0, bert, 4) != 4 || bert[0]!='b' || bert[1]!='e' || bert[2] !='r' || bert[3]!='t') { cerr<<"EOF or bad magic"< nmm.gi().gnsstow()) { @@ -1146,7 +1218,6 @@ try auto newOffset = bm.getAtomicOffset(bm.sow); svstat.timeDisco = oldOffset.first - newOffset.first; idb.addValue(id, "clock_jump_ns", svstat.timeDisco); - } svstat.lastBeidouMessage1 = bm; } @@ -1182,6 +1253,18 @@ try if(bm.sqrtA) // only copy in if complete svstat.oldBeidouMessage = bm; } + else if((fraid == 4 && 1<= pageno && pageno <= 24) || + (fraid == 5 && 1<= pageno && pageno <= 6) || + (fraid == 5 && 11<= pageno && pageno <= 23) ) { + + struct BeidouAlmanacEntry bae; + // bm.alma.AmEpID = svstat.oldBeidouMessage.alma.AmEpID; // this comes from older messages + + if(processBeidouAlmanac(bm, bae)) { + g_beidoualma[bae.sv]=bae; + } + } + if(fraid==5 && pageno == 9) { svstat.a0g = bm.a0gps; svstat.a1g = bm.a1gps; @@ -1193,10 +1276,10 @@ try g_dtLSBeidou = bm.deltaTLS; // cout<<"Beidou leap seconds: "<