From 2829ca9cd860c36948a28f30b2a52f6b7fc10a45 Mon Sep 17 00:00:00 2001 From: bert hubert Date: Sat, 29 Feb 2020 14:28:36 +0100 Subject: [PATCH] add rtcm processing, add rtcmtool --- Makefile | 13 ++-- navcat.cc | 21 ------- navdump.cc | 18 ++++++ navmon.cc | 20 +++++++ navmon.hh | 1 + navmon.proto | 9 ++- navparse.cc | 43 +++++++++++++ rtcm.cc | 111 ++++++++++++++++++++++++++++++++++ rtcm.hh | 49 +++++++++++++++ rtcmtool.cc | 166 +++++++++++++++++++++++++++++++++++++++++++++++++++ ubxtool.cc | 163 +------------------------------------------------- 11 files changed, 426 insertions(+), 188 deletions(-) create mode 100644 rtcm.cc create mode 100644 rtcm.hh create mode 100644 rtcmtool.cc diff --git a/Makefile b/Makefile index c238526..034cd14 100644 --- a/Makefile +++ b/Makefile @@ -25,7 +25,7 @@ endif CHEAT_ARG := $(shell ./update-git-hash-if-necessary) PROGRAMS = navparse ubxtool navnexus navcat navrecv navdump testrunner navdisplay tlecatch reporter \ - galmonmon rinreport + galmonmon rinreport rtcmtool all: navmon.pb.cc $(PROGRAMS) @@ -35,7 +35,7 @@ all: navmon.pb.cc $(PROGRAMS) navmon.pb.h: navmon.proto protoc --cpp_out=./ navmon.proto - + navmon.pb.cc: navmon.proto protoc --cpp_out=./ navmon.proto @@ -74,7 +74,7 @@ download-raspbian-package: decrypt: decrypt.o bits.o ext/fmt-6.1.2/src/format.o $(CXX) -std=gnu++17 $^ -o $@ -navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o sbas.o +navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o sbas.o rtcm.o $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY) reporter: reporter.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o githash.o @@ -84,7 +84,7 @@ galmonmon: galmonmon.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ub $(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lprotobuf -lcurl -navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o sbas.o ${EXTRADEP} +navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o sbas.o rtcm.o ${EXTRADEP} $(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -pthread -lprotobuf -lz navdisplay: navdisplay.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o osen.o githash.o @@ -107,8 +107,11 @@ tlecatch: tlecatch.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) gith rinreport: rinreport.o rinex.o githash.o navmon.o ext/fmt-6.1.2/src/format.o ephemeris.o osen.o $(CXX) -std=gnu++17 $^ -o $@ -lz -pthread +rtcmtool: rtcmtool.o navmon.pb.o githash.o ext/fmt-6.1.2/src/format.o bits.o nmmsender.o $(SIMPLESOCKETS) navmon.o rtcm.o + $(CXX) -std=gnu++17 $^ -o $@ -lz -pthread -lprotobuf -ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-6.1.2/src/format.o galileo.o gps.o beidou.o navmon.o ephemeris.o $(SIMPLESOCKETS) osen.o githash.o + +ubxtool: navmon.pb.o ubxtool.o ubx.o bits.o ext/fmt-6.1.2/src/format.o galileo.o gps.o beidou.o navmon.o ephemeris.o $(SIMPLESOCKETS) osen.o githash.o nmmsender.o $(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -lprotobuf -pthread testrunner: navmon.pb.o testrunner.o ubx.o bits.o ext/fmt-6.1.2/src/format.o galileo.o gps.o beidou.o ephemeris.o sp3.o osen.o navmon.o rinex.o githash.o diff --git a/navcat.cc b/navcat.cc index edef136..eb0e793 100644 --- a/navcat.cc +++ b/navcat.cc @@ -81,27 +81,6 @@ vector getSources(string_view dirname) return ret; } -static size_t writen2(int fd, const void *buf, size_t count) -{ - const char *ptr = (char*)buf; - const char *eptr = ptr + count; - - ssize_t res; - while(ptr != eptr) { - res = ::write(fd, ptr, eptr - ptr); - if(res < 0) { - throw runtime_error("failed in writen2: "+string(strerror(errno))); - } - else if (res == 0) - throw EofException(); - - ptr += (size_t) res; - } - - return count; -} - - void sendProtobuf(string_view dir, time_t startTime, time_t stopTime=0) { diff --git a/navdump.cc b/navdump.cc index e1c44c0..cab4956 100644 --- a/navdump.cc +++ b/navdump.cc @@ -30,6 +30,7 @@ #include "version.hh" #include "gpscnav.hh" #include "rinex.hh" +#include "rtcm.hh" static char program[]="navdump"; @@ -646,6 +647,23 @@ try cout<<"\n"; } + else if(nmm.type() == NavMonMessage::RTCMMessageType) { + etstamp(); + RTCMMessage rm; + rm.parse(nmm.rm().contents()); + if(rm.type == 1057 || rm.type == 1240) { + for(const auto& ed : rm.d_ephs) { + cout< ("; + cout<< ed.dradial<<", "< + +using namespace std; + +void RTCMMessage::parse(const std::string& str) +{ + auto gbu=[&str](int offset, int bits) { + return getbitu((const unsigned char*)str.c_str(), offset, bits); + }; + auto gbs=[&str](int offset, int bits) { + return getbits((const unsigned char*)str.c_str(), offset, bits); + }; + + type = gbu(0, 12); + // cout<<"Message number: "< ("; + // cout<< ed.dradial<<", "< +#include +#include "navmon.hh" +#include +struct RTCMFrame +{ + std::string payload; +}; + +class RTCMReader +{ +public: + explicit RTCMReader(int fd) : d_fp(fdopen(fd, "r")) {} + bool get(RTCMFrame& rf); +private: + FILE* d_fp; +}; + + +struct RTCMMessage +{ + void parse(const std::string& str); + int type; + int sow; + int udi; + bool mmi; + bool reference; + int ssrIOD, ssrProvider, ssrSolution; + struct EphemerisDelta + { + SatID id; + // in millimeters + double radial, along, cross; + double dradial, dalong, dcross; + int iod; + }; + struct ClockDelta + { + SatID id; + double dclock0; // in meters + double dclock1; + double dclock2; + }; + + std::vector d_ephs; + std::vector d_clocks; + +}; diff --git a/rtcmtool.cc b/rtcmtool.cc new file mode 100644 index 0000000..7d36f49 --- /dev/null +++ b/rtcmtool.cc @@ -0,0 +1,166 @@ +#include "rtcm.hh" +#include "bits.hh" +#include +#include +#include "nmmsender.hh" +#include "CLI/CLI.hpp" +#include "swrappers.hh" +#include "sclasses.hh" +#include "version.hh" + +using namespace std; + +bool RTCMReader::get(RTCMFrame& rf) +{ + int c; + while( ((c=fgetc(d_fp)) != -1) && c != 211) { + cerr<<"Skipped.. "< buf(size); + if((int)fread(&buf[0], 1, size, d_fp) != size) + return false; + + // cout<<"Returning true"< destinations; + + bool doVERSION{false}, doSTDOUT{false}; + CLI::App app(program); + app.add_option("--destination,-d", destinations, "Send output to this IPv4/v6 address"); + app.add_option("--station", g_srcid, "Station id")->required(); + app.add_flag("--version", doVERSION, "show program version and copyright"); + app.add_flag("--stdout", doSTDOUT, "Emit output to stdout"); + try { + app.parse(argc, argv); + } catch(const CLI::Error &e) { + return app.exit(e); + } + + if(doVERSION) { + showVersion(program, g_gitHash); + exit(0); + } + + + signal(SIGPIPE, SIG_IGN); + NMMSender ns; + ns.d_debug = true; + for(const auto& s : destinations) { + auto res=resolveName(s, true, true); + if(res.empty()) { + cerr<<"Unable to resolve '"<set_contents(rf.payload); + ns.emitNMM(nmm); + + // rm.parse(rf.payload); + } +} diff --git a/ubxtool.cc b/ubxtool.cc index a1d69cb..af31853 100644 --- a/ubxtool.cc +++ b/ubxtool.cc @@ -33,7 +33,7 @@ #include "comboaddress.hh" #include "swrappers.hh" #include "sclasses.hh" - +#include "nmmsender.hh" #include "version.hh" static char program[]="ubxtool"; @@ -70,25 +70,6 @@ static int getBaudrate(int baud) static int g_baudval; -size_t writen2(int fd, const void *buf, size_t count) -{ - const char *ptr = (char*)buf; - const char *eptr = ptr + count; - - ssize_t res; - while(ptr != eptr) { - res = ::write(fd, ptr, eptr - ptr); - if(res < 0) { - throw runtime_error("failed in writen2: "+string(strerror(errno))); - } - else if (res == 0) - throw EofException(); - - ptr += (size_t) res; - } - - return count; -} /* inav schedule: 1) Find plausible start time of next cycle @@ -294,147 +275,6 @@ bool sendAndWaitForUBXAckNack(int fd, int seconds, basic_string_view ms } -class NMMSender -{ - struct Destination - { - int fd{-1}; - std::string dst; - std::string fname; - - deque queue; - std::mutex mut; - void emitNMM(const NavMonMessage& nmm); - }; - -public: - void addDestination(int fd) - { - auto d = std::make_unique(); - d->fd = fd; - d_dests.push_back(std::move(d)); - } - void addDestination(const std::string& dest) - { - auto d = std::make_unique(); - d->dst = dest; - d_dests.push_back(std::move(d)); - } - - void launch() - { - for(auto& d : d_dests) { - if(!d->dst.empty()) { - thread t(&NMMSender::sendTCPThread, this, d.get()); - t.detach(); - } - } - } - - void sendTCPThread(Destination* d) - { - struct NameError{}; - for(;;) { - ComboAddress chosen; - try { - vector addrs; - for(;;) { - addrs=resolveName(d->dst, true, true); - if(!addrs.empty()) - break; - - cerr<dst<<", sleeping and trying again later"<dst<<" on "< mut(d->mut); - if(!d->queue.empty()) { - msg = d->queue.front(); - } - } - if(!msg.empty()) { - sc.writen(msg); - std::lock_guard mut(d->mut); - d->queue.pop_front(); - } - else usleep(100000); - } - } - } - catch(NameError&) { - { - std::lock_guard mut(d->mut); - if (doDEBUG) { cerr<queue.size()<<" messages queued for "<dst<dst<<" via "< mut(d->mut); - if (doDEBUG) { cerr<queue.size()<<" messages queued for "<dst<dst <<" via "< mut(d->mut); - if (doDEBUG) { cerr<<"There are now "<queue.size()<<" messages queued for "<dst<<" via "<> d_dests; -}; - -void NMMSender::emitNMM(const NavMonMessage& nmm) -{ - for(auto& d : d_dests) { - d->emitNMM(nmm); - } -} - -void NMMSender::Destination::emitNMM(const NavMonMessage& nmm) -{ - string out; - nmm.SerializeToString(& out); - string msg("bert"); - - uint16_t len = htons(out.size()); - msg.append((char*)&len, 2); - msg.append(out); - - if(!dst.empty()) { - std::lock_guard l(mut); - queue.push_back(msg); - } - else - writen2(fd, msg.c_str(), msg.size()); -} - bool version9 = false; void enableUBXMessageOnPort(int fd, uint8_t ubxClass, uint8_t ubxType, uint8_t port, uint8_t rate=1) @@ -641,6 +481,7 @@ int main(int argc, char** argv) signal(SIGPIPE, SIG_IGN); NMMSender ns; + ns.d_debug = doDEBUG; for(const auto& s : destinations) { auto res=resolveName(s, true, true); if(res.empty()) {