trkmeas work, plus many other things
bert hubert
parent
a58a9f139f
commit
9dd4b4232d
24
Makefile
24
Makefile
|
|
@ -1,3 +1,5 @@
|
|||
CFLAGS = -O3 -Wall -ggdb
|
||||
|
||||
CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/CLI11 \
|
||||
-Iext/fmt-5.2.1/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ \
|
||||
-I/usr/local/opt/openssl/include/ \
|
||||
|
|
@ -6,10 +8,17 @@ CXXFLAGS:= -std=gnu++17 -Wall -O3 -MMD -MP -ggdb -fno-omit-frame-pointer -Iext/C
|
|||
|
||||
# CXXFLAGS += -Wno-delete-non-virtual-dtor
|
||||
|
||||
ifneq (,$(wildcard ubxsec.c))
|
||||
EXTRADEP = ubxsec.o
|
||||
else ifneq (,$(wildcard ubxsec.o))
|
||||
EXTRADEP = ubxsec.o
|
||||
endif
|
||||
|
||||
|
||||
CHEAT_ARG := $(shell ./update-git-hash-if-necessary)
|
||||
|
||||
PROGRAMS = navparse ubxtool navnexus navcat navrecv navdump testrunner navdisplay tlecatch reporter
|
||||
PROGRAMS = navparse ubxtool navnexus navcat navrecv navdump testrunner navdisplay tlecatch reporter \
|
||||
galmonmon
|
||||
|
||||
all: navmon.pb.cc $(PROGRAMS)
|
||||
|
||||
|
|
@ -24,15 +33,20 @@ clean:
|
|||
rm -f *~ *.o *.d ext/*/*.o $(PROGRAMS) navmon.pb.h navmon.pb.cc $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) $(H2OPP) $(SIMPLESOCKETS)
|
||||
rm -f ext/fmt-5.2.1/src/format.o
|
||||
|
||||
decrypt: decrypt.o bits.o ext/fmt-5.2.1/src/format.o
|
||||
$(CXX) -std=gnu++17 $^ -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 osen.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 osen.o trkmeas.o ${EXTRADEP}
|
||||
$(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-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 osen.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-5.2.1/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
|
||||
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lprotobuf -lcurl
|
||||
|
||||
galmonmon: galmonmon.o ext/fmt-5.2.1/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
|
||||
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lprotobuf -lcurl
|
||||
|
||||
|
||||
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 osen.o
|
||||
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 osen.o trkmeas.o ${EXTRADEP}
|
||||
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -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 ephemeris.o navmon.o osen.o
|
||||
|
|
|
|||
|
|
@ -0,0 +1,290 @@
|
|||
|
||||
#include "minicurl.hh"
|
||||
#include <iostream>
|
||||
#include "navmon.hh"
|
||||
#include "fmt/format.h"
|
||||
#include "fmt/printf.h"
|
||||
#include "ext/powerblog/h2o-pp.hh"
|
||||
#include <variant>
|
||||
#include "githash.h"
|
||||
using namespace std;
|
||||
|
||||
/*
|
||||
Monitoring the satellites for sensible alerts.
|
||||
|
||||
A satellite transitions state if:
|
||||
* galmon is up
|
||||
* galmon is up to date
|
||||
* We are seeing recent messages for the satellite
|
||||
* For more than 60 seconds this state is different than it was
|
||||
|
||||
A satellite becomes 'unobserved' if:
|
||||
* galmon is up
|
||||
* galmon is up to date
|
||||
* We haven't seen a message since an hour
|
||||
|
||||
A satellite becomes observed again if we have seen it for > 60 seconds.
|
||||
|
||||
At startup, every satellite assumes, without alert, the status we find for it, which is not a transition.
|
||||
|
||||
*/
|
||||
|
||||
|
||||
class StateKeeper
|
||||
{
|
||||
public:
|
||||
typedef std::variant<bool, double, string> var_t;
|
||||
void setBoolNames(string_view name, string_view offName, string_view onName);
|
||||
std::optional<string> reportState(string_view thing, string_view name, var_t state, time_t now=0);
|
||||
std::optional<string> getState(string_view thing, string_view name);
|
||||
std::optional<string> getPrevState(string_view thing, string_view name);
|
||||
|
||||
private:
|
||||
struct Names
|
||||
{
|
||||
string offName, onName;
|
||||
};
|
||||
map<string, Names> names;
|
||||
|
||||
struct State
|
||||
{
|
||||
var_t state;
|
||||
time_t since;
|
||||
};
|
||||
|
||||
struct ThingState
|
||||
{
|
||||
std::optional<State> prev, live, provisional;
|
||||
};
|
||||
|
||||
map<string, map<string, ThingState> > states;
|
||||
private:
|
||||
std::string getName(string_view name, var_t state)
|
||||
{
|
||||
if(auto ptr = std::get_if<bool>(&state)) {
|
||||
if(*ptr) {
|
||||
return names[(string)name].onName;
|
||||
}
|
||||
else
|
||||
return names[(string)name].offName;
|
||||
}
|
||||
if(auto ptr = std::get_if<string>(&state)) {
|
||||
return *ptr;
|
||||
}
|
||||
if(auto ptr = std::get_if<double>(&state)) {
|
||||
return to_string(*ptr);
|
||||
}
|
||||
return "?";
|
||||
}
|
||||
};
|
||||
|
||||
void StateKeeper::setBoolNames(string_view name, string_view offName, string_view onName)
|
||||
{
|
||||
names[(string)name].offName = offName;
|
||||
names[(string)name].onName = onName;
|
||||
}
|
||||
|
||||
std::optional<string> StateKeeper::getState(string_view thing, string_view name)
|
||||
{
|
||||
if(states.count((string)thing) && states[(string)thing].count((string) name) && states[(string)thing][(string)name].live) {
|
||||
return getName(name, states[(string)thing][(string)name].live->state);
|
||||
}
|
||||
return std::optional<string>();
|
||||
}
|
||||
|
||||
std::optional<string> StateKeeper::getPrevState(string_view thing, string_view name)
|
||||
{
|
||||
if(states.count((string)thing) && states[(string)thing].count((string) name) && states[(string)thing][(string)name].prev) {
|
||||
return getName(name, states[(string)thing][(string)name].prev->state);
|
||||
}
|
||||
return std::optional<string>();
|
||||
}
|
||||
|
||||
|
||||
std::optional<string> StateKeeper::reportState(string_view thing, string_view name, var_t newstate, time_t now)
|
||||
{
|
||||
auto& state = states[(string)thing][(string)name];
|
||||
std::optional<string> ret;
|
||||
|
||||
if(!now)
|
||||
now = time(0);
|
||||
|
||||
if(!state.live) { // we had no state yet
|
||||
state.live = State{newstate, now};
|
||||
state.provisional.reset(); // for good measure
|
||||
return ret;
|
||||
}
|
||||
else if(state.live->state == newstate) { // confirmation of current state
|
||||
state.provisional.reset();
|
||||
return ret;
|
||||
}
|
||||
else if(!state.provisional) { // new provisional state
|
||||
state.provisional = State{newstate, now};
|
||||
return ret;
|
||||
}
|
||||
else {
|
||||
if(now - state.provisional->since > 60) { // provisional state has been confirmed
|
||||
state.prev = state.live;
|
||||
state.live = state.provisional;
|
||||
state.provisional.reset();
|
||||
return getName(name, state.live->state);
|
||||
}
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
StateKeeper g_sk;
|
||||
|
||||
static std::string string_replace(const std::string& str, const std::string& match,
|
||||
const std::string& replacement, unsigned int max_replacements = UINT_MAX)
|
||||
{
|
||||
size_t pos = 0;
|
||||
std::string newstr = str;
|
||||
unsigned int replacements = 0;
|
||||
while ((pos = newstr.find(match, pos)) != std::string::npos
|
||||
&& replacements < max_replacements)
|
||||
{
|
||||
newstr.replace(pos, match.length(), replacement);
|
||||
pos += replacement.length();
|
||||
replacements++;
|
||||
}
|
||||
return newstr;
|
||||
}
|
||||
|
||||
void sendTweet(const string& tweet)
|
||||
{
|
||||
string etweet = string_replace(tweet, "+", "%2b");
|
||||
system((string("twurl -X POST \"/1.1/statuses/update.json?status=")+etweet+"\" >> twitter.log").c_str());
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
MiniCurl mc;
|
||||
MiniCurl::MiniCurlHeaders mch;
|
||||
// string url="https://galmon.eu/svs.json";
|
||||
string url="http://[::1]:10000/";
|
||||
|
||||
g_sk.setBoolNames("health", "healthy", "unhealthy");
|
||||
g_sk.setBoolNames("eph-too-old", "ephemeris fresh", "ephemeris too old");
|
||||
g_sk.setBoolNames("silent", "observed", "not observed");
|
||||
|
||||
std::variant<bool, string> tst;
|
||||
|
||||
extern const char* g_gitHash;
|
||||
|
||||
auto observers = nlohmann::json::parse(mc.getURL(url+"observers.json"));
|
||||
|
||||
cout<<("Galmonmon " +string(g_gitHash)+ " started, " + std::to_string(observers.size()) +" observers seen")<<endl;
|
||||
|
||||
for(;;) {
|
||||
try {
|
||||
auto res = mc.getURL(url+"global.json");
|
||||
auto j = nlohmann::json::parse(res);
|
||||
if(!j.count("last-seen") || time(0) - (long) j["last-seen"] > 30) {
|
||||
cout<<"Galmon at "<<url<<" is not current"<<endl;
|
||||
sleep(10);
|
||||
continue;
|
||||
}
|
||||
/*
|
||||
if(auto iter = j.find("last-seen"); iter != j.end()) {
|
||||
cout<<"Galmon behind by " << (time(0) - (long) iter.value()) <<" seconds"<<endl;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
res = mc.getURL(url+"svs.json");
|
||||
j = nlohmann::json::parse(res);
|
||||
bool first=true;
|
||||
for(const auto& sv : j) {
|
||||
|
||||
int gnssid = sv["gnssid"];
|
||||
string fullName = sv["fullName"];
|
||||
|
||||
if(!(gnssid == 2 && sv["sigid"]==1) &&
|
||||
!(gnssid == 0 && sv["sigid"]==0) &&
|
||||
!(gnssid == 3 && sv["sigid"]==0) &&
|
||||
!(gnssid == 6 && sv["sigid"]==0))
|
||||
continue;
|
||||
|
||||
int numfresh=0;
|
||||
// we only track "received status" for GPS and Galileo
|
||||
bool notseen= gnssid ==0 || gnssid==2;
|
||||
if(!sv.count("sisa") && !sv.count("eph-age-m"))
|
||||
continue;
|
||||
if(sv.count("perrecv")) {
|
||||
for(const auto& recv : sv["perrecv"]) {
|
||||
if((int)recv["last-seen-s"] < 60)
|
||||
numfresh++;
|
||||
if((int)recv["last-seen-s"] < 3600)
|
||||
notseen=false;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
auto healthchange = g_sk.reportState(fullName, "health", sv["healthissue"]!=0);
|
||||
std::optional<string> tooOldChange;
|
||||
if(gnssid == 2)
|
||||
tooOldChange = g_sk.reportState(fullName, "eph-too-old", sv["eph-age-m"] > 120);
|
||||
else
|
||||
tooOldChange = g_sk.reportState(fullName, "eph-too-old", sv["eph-age-m"] > 140);
|
||||
|
||||
auto seenChange = g_sk.reportState(fullName, "silent", notseen);
|
||||
|
||||
auto sisaChange = g_sk.reportState(fullName, "sisa", (string)sv["sisa"]);
|
||||
|
||||
/*
|
||||
cout<<fullName<<": numfresh "<<numfresh << " healthissue "<<sv["healthissue"];
|
||||
cout<<" eph-age-m "<< fmt::sprintf("%.2f", (double)sv["eph-age-m"]);
|
||||
cout<<" not-seen "<<notseen;
|
||||
if(auto val = g_sk.getState(fullName, "eph-too-old"); val) {
|
||||
cout<<" eph-too-old \""<<*val<<"\"";
|
||||
}
|
||||
if(auto val = g_sk.getState(fullName, "health"); val) {
|
||||
cout<<" health \""<<*val<<"\"";
|
||||
}
|
||||
*/
|
||||
if(healthchange || tooOldChange || seenChange || sisaChange) {
|
||||
if(first)
|
||||
cout<<"\n";
|
||||
first=false;
|
||||
ostringstream out;
|
||||
|
||||
if(gnssid == 0)
|
||||
out<<"GPS";
|
||||
else if(gnssid == 2)
|
||||
out<<"Galileo";
|
||||
else if(gnssid == 3)
|
||||
out<<"BeiDou";
|
||||
else if(gnssid== 6)
|
||||
out<<"GLONASS";
|
||||
out<<" "<<fullName<<": ";
|
||||
|
||||
if(healthchange)
|
||||
out<< *healthchange<<" ";
|
||||
if(tooOldChange) {
|
||||
out<< *tooOldChange<<", new value: "<< fmt::sprintf("%.02f", (double)sv["eph-age-m"])<<" minutes, old: ";
|
||||
out<< *g_sk.getPrevState(fullName, "eph-too-old");
|
||||
}
|
||||
if(seenChange)
|
||||
out<< *seenChange<<" ";
|
||||
if(sisaChange) {
|
||||
out<< "SISA/URA reported ranging accuracy new: "<<*sisaChange<<", old: " << *g_sk.getPrevState(fullName, "sisa");
|
||||
}
|
||||
if(out.str().find("200 cm") == string::npos || out.str().find("282 cm") == string::npos)
|
||||
sendTweet("TESTING: "+out.str());
|
||||
cout<<humanTimeNow()<<" CHANGE ";
|
||||
cout<<out.str()<<endl;
|
||||
|
||||
}
|
||||
}
|
||||
cout<<".";
|
||||
cout.flush();
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
cerr<<"\nError: "<<e.what()<<endl;
|
||||
}
|
||||
sleep(20);
|
||||
}
|
||||
|
||||
}
|
||||
315
navdump.cc
315
navdump.cc
|
|
@ -24,7 +24,7 @@
|
|||
#include "navmon.hh"
|
||||
#include "tle.hh"
|
||||
#include "sp3.hh"
|
||||
|
||||
#include "ubx.hh"
|
||||
#include <unistd.h>
|
||||
using namespace std;
|
||||
|
||||
|
|
@ -145,6 +145,82 @@ struct SVFilter
|
|||
|
||||
};
|
||||
|
||||
struct FixStat
|
||||
{
|
||||
double iTow{-1};
|
||||
|
||||
struct SatStat
|
||||
{
|
||||
double bestrange1{-1}; // corrected for clock
|
||||
double bestrange5{-1}; // corrected for clock
|
||||
double doppler1{-1}; // corrected for clock
|
||||
double doppler5{-1}; // corrected for clock
|
||||
double radvel{-1};
|
||||
double ephrange{-1};
|
||||
GalileoMessage ephemeris;
|
||||
};
|
||||
|
||||
map<pair<int,int>, SatStat> sats;
|
||||
};
|
||||
|
||||
double g_rcvoffset;
|
||||
void emitFixState(int src, double iTow, FixStat& fs, int n)
|
||||
{
|
||||
cout<<"\nFix state for source "<<src<<", have "<<fs.sats.size()<<" satellites, n="<<n<<endl;
|
||||
// for(double dt = -0.2; dt < 0.2; dt+=0.0001)
|
||||
double dt=0;
|
||||
double offset=0;
|
||||
{
|
||||
|
||||
int count=0;
|
||||
for(const auto& s : fs.sats) {
|
||||
if(s.first.second==14 || s.first.second==18)
|
||||
continue;
|
||||
|
||||
Point sat;
|
||||
double E=getCoordinates(iTow, s.second.ephemeris, &sat);
|
||||
if(getElevationDeg(sat, g_ourpos) < 20)
|
||||
continue;
|
||||
/*
|
||||
Point sat;
|
||||
auto [toffset, trend] = s.second.ephemeris.getAtomicOffset(iTow +dt);
|
||||
(void)trend;
|
||||
|
||||
getCoordinates(iTow + toffset/1000000000.0 + dt, s.second.ephemeris, &sat);
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
|
||||
getCoordinates(iTow + range/299792458.0 + toffset/1000000000.0 + dt, s.second.ephemeris, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
*/
|
||||
double range = s.second.ephrange;
|
||||
if(s.second.bestrange1 != -1) {
|
||||
offset += s.second.bestrange1 - range;
|
||||
count++;
|
||||
}
|
||||
if(s.second.bestrange5 != -1) {
|
||||
offset += s.second.bestrange5 - range;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
if(!count) {
|
||||
fs.sats.clear();
|
||||
return;
|
||||
}
|
||||
|
||||
cout<< " dt "<<dt<<" err "<<offset << " " << count << " avg " << offset/count<< " "<<offset/count/3e5<<"ms"<<endl;
|
||||
offset/=count;
|
||||
}
|
||||
|
||||
for(const auto& s : fs.sats) {
|
||||
Point sat;
|
||||
double E=getCoordinates(iTow, s.second.ephemeris, &sat);
|
||||
cout<<""<<s.first.first<<","<<s.first.second<<": "<<s.second.bestrange1-offset<<" "<<s.second.bestrange5-offset<<" " << s.second.ephrange<<", delta1 " << (s.second.bestrange1-offset-s.second.ephrange)<<", delta5 "<< (s.second.bestrange5-offset-s.second.ephrange)<<" dd "<< s.second.bestrange1 - s.second.bestrange5 <<" t0e " << s.second.ephemeris.getT0e()<< " elev " << getElevationDeg(sat, g_ourpos) << " E " << E<< " clock " << s.second.ephemeris.getAtomicOffset(iTow).first/1000000<<"ms doppler1 "<<s.second.doppler1 << " doppler5 " <<s.second.doppler5<<" radvel " <<s.second.radvel<< " frac " << (s.second.bestrange1-offset-s.second.ephrange)/s.second.radvel;
|
||||
cout<< " fixed "<<((s.second.bestrange1 - offset-s.second.ephrange) + (s.second.ephrange/299792458.0) * s.second.radvel)<< " BGD-ns "<<ldexp(s.second.ephemeris.BGDE1E5b,-32)*1000000000<< endl;
|
||||
}
|
||||
|
||||
fs.sats.clear();
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
try
|
||||
{
|
||||
|
|
@ -167,10 +243,12 @@ try
|
|||
}
|
||||
*/
|
||||
vector<string> svpairs;
|
||||
vector<int> stations;
|
||||
bool doReceptionData{false};
|
||||
bool doRFData{true};
|
||||
bool doObserverPosition{false};
|
||||
app.add_option("--svs", svpairs, "Listen to specified svs. '0' = gps, '2' = Galileo, '2,1' is E01");
|
||||
app.add_option("--stations", stations, "Listen to specified stations.");
|
||||
app.add_option("--positions,-p", doObserverPosition, "Print out observer positions (or not)");
|
||||
app.add_option("--rfdata,-r", doRFData, "Print out RF data (or not)");
|
||||
|
||||
|
|
@ -183,6 +261,10 @@ try
|
|||
for(const auto& svp : svpairs) {
|
||||
svfilter.addFilter(svp);
|
||||
}
|
||||
|
||||
set<int> statset;
|
||||
for(const auto& i : stations)
|
||||
statset.insert(i);
|
||||
|
||||
ofstream almanac("almanac.txt");
|
||||
ofstream iodstream("iodstream.csv");
|
||||
|
|
@ -194,7 +276,13 @@ try
|
|||
ofstream sp3csv;
|
||||
|
||||
sp3csv.open ("sp3.csv", std::ofstream::out | std::ofstream::app);
|
||||
sp3csv<<"timestamp gnssid sv ephAge sp3X sp3Y sp3Z ephX ephY ephZ sp3Clock ephClock distance clockDelta"<<endl;
|
||||
|
||||
sp3csv<<"timestamp gnssid sv ephAge sp3X sp3Y sp3Z ephX ephY ephZ sp3Clock ephClock distance along clockDelta E speed"<<endl;
|
||||
|
||||
ofstream loccsv;
|
||||
loccsv.open ("jeff.csv", std::ofstream::out | std::ofstream::app);
|
||||
//loccsv<<"timestamp lat lon altitude accuracy\n";
|
||||
|
||||
|
||||
for(;;) {
|
||||
char bert[4];
|
||||
|
|
@ -220,6 +308,9 @@ try
|
|||
NavMonMessage nmm;
|
||||
nmm.ParseFromString(string(buffer, len));
|
||||
|
||||
if(!statset.empty() && !statset.count(nmm.sourceid()))
|
||||
continue;
|
||||
|
||||
// if(nmm.type() == NavMonMessage::ReceptionDataType)
|
||||
// continue;
|
||||
|
||||
|
|
@ -232,7 +323,7 @@ try
|
|||
cout<< "imptow "<<imptow-2<<" ";
|
||||
};
|
||||
|
||||
|
||||
static map<int,GalileoMessage> galEphemeris;
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
if(doReceptionData) {
|
||||
etstamp();
|
||||
|
|
@ -273,6 +364,7 @@ try
|
|||
// 2^-34 2^-46
|
||||
cout <<" iodnav "<<gm.iodnav <<" af0 "<<gm.af0 <<" af1 "<<gm.af1 <<", scaled: "<<ldexp(1.0*gm.af0, 19-34)<<", "<<ldexp(1.0*gm.af1, 38-46);
|
||||
if(tow && oldgm4s.count(nmm.gi().gnsssv()) && oldgm4s[nmm.gi().gnsssv()].iodnav != gm.iodnav) {
|
||||
|
||||
auto& oldgm4 = oldgm4s[nmm.gi().gnsssv()];
|
||||
auto oldOffset = oldgm4.getAtomicOffset(tow);
|
||||
auto newOffset = gm.getAtomicOffset(tow);
|
||||
|
|
@ -286,6 +378,7 @@ try
|
|||
gmwtypes[{sv,3}].iodnav == gmwtypes[{sv,4}].iodnav) {
|
||||
cout <<" have complete ephemeris at " << gm.iodnav;
|
||||
|
||||
galEphemeris[sv] = gm;
|
||||
|
||||
int start = utcFromGST(gm.wn, gm.tow);
|
||||
|
||||
|
|
@ -296,9 +389,16 @@ try
|
|||
if(!haveSeen.count({sv, bestSP3->t})) {
|
||||
haveSeen.insert({sv, bestSP3->t});
|
||||
Point newPoint;
|
||||
getCoordinates(gm.tow + (bestSP3->t - start), gm, &newPoint);
|
||||
double E=getCoordinates(gm.tow + (bestSP3->t - start), gm, &newPoint, false);
|
||||
Point sp3Point(bestSP3->x, bestSP3->y, bestSP3->z);
|
||||
Vector dist(newPoint, sp3Point);
|
||||
|
||||
Vector nspeed;
|
||||
getSpeed(gm.tow + (bestSP3->t - start), gm, &nspeed);
|
||||
Vector speed = nspeed;
|
||||
nspeed.norm();
|
||||
double along = nspeed.inner(dist);
|
||||
|
||||
cout<<"\nsp3 "<<(bestSP3->t - start)<<" E"<<sv<<" "<<humanTime(bestSP3->t)<<" (" << newPoint.x/1000.0 <<", "<<newPoint.y/1000.0<<", "<<newPoint.z/1000.0<< ") (" <<
|
||||
(bestSP3->x/1000.0) <<", " << (bestSP3->y/1000.0) <<", " << (bestSP3->z/1000.0) << ") "<<bestSP3->clockBias << " " << gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first<< " " << dist.length()<< " ";
|
||||
cout << (bestSP3->clockBias - gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first);
|
||||
|
|
@ -306,8 +406,8 @@ try
|
|||
cout << endl;
|
||||
|
||||
sp3csv <<std::fixed<< bestSP3->t << " 2 "<< sv <<" " << ephAge(gm.tow+(bestSP3->t - start), gm.getT0e()) <<" "<<bestSP3->x<<" " << bestSP3->y<<" " <<bestSP3->z <<" " << newPoint.x<<" " <<newPoint.y <<" " <<newPoint.z << " " <<bestSP3->clockBias <<" ";
|
||||
sp3csv << gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first<<" " << dist.length() <<" ";
|
||||
sp3csv << (bestSP3->clockBias - gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first) << endl;
|
||||
sp3csv << gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first<<" " << dist.length() <<" " << along <<" ";
|
||||
sp3csv << (bestSP3->clockBias - gm.getAtomicOffset(gm.tow + (bestSP3->t-start)).first) << " " << E << " " << speed.length()<<endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -626,6 +726,7 @@ try
|
|||
cout<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
g_ourpos = Point(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
if(!doObserverPosition)
|
||||
continue;
|
||||
etstamp();
|
||||
|
|
@ -635,16 +736,25 @@ try
|
|||
cout<<", WGS84 lon "<< 180*std::get<1>(latlonh)/M_PI
|
||||
<<" lat "<< 180*std::get<0>(latlonh)/M_PI
|
||||
<<" elev "<< std::get<2>(latlonh) << " acc "<<nmm.op().acc()<<" m "<<endl;
|
||||
g_ourpos = Point(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
|
||||
loccsv<<std::fixed<<nmm.localutcseconds()+nmm.localutcnanoseconds()/1000000000.0<<" "<<180*std::get<1>(latlonh)/M_PI<<" "<<
|
||||
180*std::get<0>(latlonh)/M_PI<<" "<<std::get<2>(latlonh)<<" "<<nmm.op().acc()<<"\n";
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
if(!doRFData)
|
||||
continue;
|
||||
|
||||
if(!svfilter.check(nmm.rfd().gnssid(), nmm.rfd().gnsssv(), nmm.gi().sigid()))
|
||||
continue;
|
||||
|
||||
|
||||
etstamp();
|
||||
cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<","<<(nmm.rfd().has_sigid() ? nmm.rfd().sigid() : 0) <<": ";
|
||||
cout<<std::fixed<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<","<<(nmm.rfd().has_sigid() ? nmm.rfd().sigid() : 0) <<": ";
|
||||
cout<<" doppler-hz " << nmm.rfd().doppler();
|
||||
cout<<" carrier-phase " << nmm.rfd().carrierphase();
|
||||
cout<<" pseudo-range " << nmm.rfd().pseudorange();
|
||||
cout<<" carrier-phase " << nmm.rfd().carrierphase();
|
||||
cout<<" rcv-tow "<<nmm.rfd().rcvtow();
|
||||
cout<<" pr-std " << nmm.rfd().prstd();
|
||||
cout<<" dop-std " << nmm.rfd().dostd();
|
||||
cout<<" cp-std " << nmm.rfd().cpstd();
|
||||
|
|
@ -652,8 +762,195 @@ try
|
|||
if(nmm.rfd().has_cno()) {
|
||||
cout<<" cno-db " <<nmm.rfd().cno();
|
||||
}
|
||||
if(nmm.rfd().has_prvalid()) {
|
||||
cout<<" prvalid " <<nmm.rfd().prvalid();
|
||||
}
|
||||
if(nmm.rfd().has_cpvalid()) {
|
||||
cout<<" cpvalid " <<nmm.rfd().cpvalid();
|
||||
}
|
||||
if(nmm.rfd().has_clkreset()) {
|
||||
cout<<" clkreset " <<nmm.rfd().clkreset();
|
||||
}
|
||||
|
||||
|
||||
static map<int,FixStat> fixes;
|
||||
|
||||
if(nmm.rfd().gnssid()==2 && galEphemeris.count(nmm.rfd().gnsssv())) { // galileo
|
||||
const auto& eph = galEphemeris[nmm.rfd().gnsssv()];
|
||||
Point sat;
|
||||
auto [offset, trend] = eph.getAtomicOffset(round(nmm.rfd().rcvtow()));
|
||||
(void)trend;
|
||||
|
||||
static int n;
|
||||
double E=getCoordinates(nmm.rfd().rcvtow(), eph, &sat);
|
||||
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
|
||||
double origrange = range;
|
||||
E=getCoordinates(nmm.rfd().rcvtow() - range/299792458.0, eph, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
cout << " d "<<origrange-range;
|
||||
origrange=range;
|
||||
/*
|
||||
E=getCoordinates(nmm.rfd().rcvtow() + range/299792458.0 + offset/1000000000.0 - 0.018, eph, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
cout << " d "<< 10000.0*(origrange-range);
|
||||
origrange=range;
|
||||
*/
|
||||
|
||||
constexpr double omegaE = 2*M_PI /86164.091 ;
|
||||
/*
|
||||
double theta = -(2*M_PI*range / 299792458.0) /86164.091; // sidereal day
|
||||
|
||||
Point rot;
|
||||
rot.x = sat.x * cos(theta) - sat.y * sin(theta);
|
||||
rot.y = sat.x * sin(theta) + sat.y * cos(theta);
|
||||
rot.z = sat.z;
|
||||
double oldrange=range;
|
||||
range = Vector(g_ourpos, rot).length(); // second try
|
||||
cout<<" rot-shift "<<oldrange-range <<" abs-move "<<Vector(rot, sat).length();
|
||||
*/
|
||||
double rotcor = omegaE * (sat.x*g_ourpos.y - sat.y * g_ourpos.x) / 299792458.0;
|
||||
cout<<" rot-shift "<<rotcor;
|
||||
range += rotcor;
|
||||
|
||||
double bestrange = nmm.rfd().pseudorange();
|
||||
double gap = range - bestrange;
|
||||
cout <<" pseudo-gap " << gap/1000.0;
|
||||
|
||||
constexpr double speedOfLightPerNS = 299792458.0 / 1000000000.0;
|
||||
bestrange += speedOfLightPerNS * offset;
|
||||
|
||||
// Δtr=F e A1/2 sin(E)
|
||||
constexpr double F = 1000000000.0*-4.442807309e-10; // "in ns"
|
||||
double dtr = F * eph.getE() * eph.getSqrtA() * sin(E);
|
||||
|
||||
bestrange -= speedOfLightPerNS * dtr;
|
||||
|
||||
cout<<" relcor "<<speedOfLightPerNS * dtr;
|
||||
|
||||
// multi-freq adjustment is done in emitFixState
|
||||
|
||||
cout<<" clockcor "<< offset/1000000.0 << "ms gap " << gap/1000.0;
|
||||
|
||||
if(fixes[nmm.sourceid()].iTow != nmm.rfd().rcvtow()) {
|
||||
emitFixState(nmm.sourceid(), nmm.rfd().rcvtow(), fixes[nmm.sourceid()], n);
|
||||
fixes[nmm.sourceid()].iTow = nmm.rfd().rcvtow();
|
||||
n++;
|
||||
}
|
||||
auto& satstat=fixes[nmm.sourceid()].sats[{(int)nmm.rfd().gnssid(), (int)nmm.rfd().gnsssv()}];
|
||||
satstat.ephrange = range;
|
||||
auto dop = doDoppler(nmm.rfd().rcvtow(), g_ourpos, eph, 1575420000);
|
||||
satstat.radvel = dop.radvel;
|
||||
if(nmm.rfd().sigid()==1) {
|
||||
satstat.bestrange1 = bestrange;
|
||||
satstat.doppler1 = nmm.rfd().doppler();
|
||||
}
|
||||
else if(nmm.rfd().sigid()==5) {
|
||||
satstat.bestrange5 = bestrange;
|
||||
satstat.doppler5 = nmm.rfd().doppler();
|
||||
}
|
||||
satstat.ephemeris = eph;
|
||||
|
||||
}
|
||||
|
||||
cout<<endl;
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverDetailsType) {
|
||||
etstamp();
|
||||
cout<<"Got observerdetails message"<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::DebuggingType) {
|
||||
etstamp();
|
||||
cout<<"Got debugging message of type "<<nmm.dm().type()<<endl;
|
||||
auto res = parseTrkMeas(basic_string<uint8_t>((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
uint64_t maxt=0;
|
||||
for(const auto& sv : res) {
|
||||
if(sv.gnss != 2) continue;
|
||||
if(sv.tr > maxt)
|
||||
maxt = sv.tr;
|
||||
}
|
||||
|
||||
double ttag = round( (ldexp(1.0*maxt, -32)/1000.0 + 0.08) / 0.1) * 0.1;
|
||||
|
||||
double rtow = round(ldexp(1.0*maxt, -32) /1000.0); // this was the rounded tow of transmission
|
||||
sort(res.begin(), res.end(), [&](const auto& a, const auto& b) {
|
||||
double elevA=0, elevB=0;
|
||||
if(galEphemeris.count(a.sv)) {
|
||||
const auto& eph = galEphemeris[a.sv];
|
||||
Point sat;
|
||||
getCoordinates(rtow, eph, &sat);
|
||||
elevA=getElevationDeg(sat, g_ourpos);
|
||||
}
|
||||
if(galEphemeris.count(b.sv)) {
|
||||
const auto& eph = galEphemeris[b.sv];
|
||||
Point sat;
|
||||
getCoordinates(rtow, eph, &sat);
|
||||
elevB=getElevationDeg(sat, g_ourpos);
|
||||
}
|
||||
return elevB < elevA;
|
||||
|
||||
});
|
||||
|
||||
|
||||
|
||||
double toffsetms=0;
|
||||
bool first = true;
|
||||
for(const auto sv : res) {
|
||||
if(sv.gnss != 2) continue;
|
||||
if(!galEphemeris.count(sv.sv))
|
||||
continue;
|
||||
|
||||
const auto& eph = galEphemeris[sv.sv];
|
||||
|
||||
double clockoffms = eph.getAtomicOffset(rtow).first/1000000.0;
|
||||
|
||||
Point sat;
|
||||
|
||||
double E=getCoordinates(rtow - clockoffms/1000.0, eph, &sat, sv.sv != 14);
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
getCoordinates(rtow - clockoffms/1000.0 - range/299792458.0, eph, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
|
||||
double trmsec = ldexp(maxt - sv.tr, -32) + clockoffms;
|
||||
|
||||
constexpr double omegaE = 2*M_PI /86164.091 ;
|
||||
double rotcor = omegaE * (sat.x*g_ourpos.y - sat.y * g_ourpos.x) / 299792458.0;
|
||||
range += rotcor;
|
||||
|
||||
double bgdcor = 299792458.0 *ldexp(eph.BGDE1E5b,-32);
|
||||
range -= bgdcor;
|
||||
|
||||
|
||||
constexpr double speedOfLightPerNS = 299792458.0 / 1000000000.0;
|
||||
// Δtr=F e A1/2 sin(E)
|
||||
constexpr double F = 1000000000.0*-4.442807309e-10; // "in ns"
|
||||
double dtr = F * eph.getE() * eph.getSqrtA() * sin(E);
|
||||
double relcor = speedOfLightPerNS * dtr;
|
||||
range -= relcor;
|
||||
|
||||
|
||||
double predTrmsec = range / 299792.4580;
|
||||
|
||||
if(first) {
|
||||
toffsetms = trmsec - predTrmsec;
|
||||
first = false;
|
||||
cout<<"Set toffsetms to "<< toffsetms << " for tow "<<rtow<<" ttag " << ttag << " raw " << (ldexp(1.0*maxt, -32) /1000.0) << endl;
|
||||
}
|
||||
|
||||
trmsec -= toffsetms;
|
||||
cout<<std::fixed<<"gnssid "<< sv.gnss<<" sv "<<sv.sv <<": doppler "<<sv.dopplerHz <<" range-ms " << range / 299792.4580;
|
||||
cout << " actual-ms " << trmsec << " delta " << ((range / 299792.4580 - trmsec))<< " delta-m " << 299792.4580*((range / 299792.4580 - trmsec));
|
||||
|
||||
cout<<" rotcor "<< rotcor;
|
||||
cout<<" relcor "<<relcor;
|
||||
cout<<" elev " << getElevationDeg(sat, g_ourpos);
|
||||
cout<<" bgd-m " << bgdcor;
|
||||
cout<<" clockoff-ms " << clockoffms << endl;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
else {
|
||||
etstamp();
|
||||
|
|
|
|||
292
navparse.cc
292
navparse.cc
|
|
@ -36,6 +36,20 @@ struct ObserverPosition
|
|||
{
|
||||
Point pos;
|
||||
double groundSpeed{-1};
|
||||
double accuracy{-1.0};
|
||||
string serialno;
|
||||
string hwversion;
|
||||
string swversion;
|
||||
string mods;
|
||||
string vendor;
|
||||
double clockOffsetNS{-1};
|
||||
double clockOffsetDriftNS{-1};
|
||||
double clockAccuracyNS{-1};
|
||||
double freqAccuracyPS{-1};
|
||||
time_t uptime;
|
||||
string githash;
|
||||
string owner;
|
||||
string remark;
|
||||
time_t lastSeen{0};
|
||||
};
|
||||
std::map<int, ObserverPosition> g_srcpos;
|
||||
|
|
@ -343,7 +357,7 @@ struct InfluxPusher
|
|||
checkSend();
|
||||
}
|
||||
|
||||
void addValueObserver(int src, string_view name, const initializer_list<pair<const char*, double>>& values, time_t t)
|
||||
void addValueObserver(int src, string_view name, const initializer_list<pair<const char*, double>>& values, time_t t, std::optional<SatID> satid=std::optional<SatID>())
|
||||
{
|
||||
if(d_mute)
|
||||
return;
|
||||
|
|
@ -358,7 +372,9 @@ struct InfluxPusher
|
|||
}
|
||||
|
||||
d_buffer+= string(name)+",src="+to_string(src);
|
||||
|
||||
if(satid) {
|
||||
d_buffer+=",gnssid="+to_string(satid->gnss)+ +",sv=" +to_string(satid->sv)+",sigid="+to_string(satid->sigid);
|
||||
}
|
||||
|
||||
d_buffer+= " ";
|
||||
bool lefirst=true;
|
||||
|
|
@ -754,8 +770,19 @@ try
|
|||
item["eph-age"] = ephAge(latestTow(2, svstats), ae.second.getT0e());
|
||||
item["i0"] = 180.0 * ae.second.getI0()/ M_PI;
|
||||
item["inclination"] = 180 * ae.second.getI0() /M_PI;
|
||||
|
||||
item["omega"] = ae.second.getOmega();
|
||||
item["sqrtA"]= ae.second.getSqrtA();
|
||||
item["M0"] = ae.second.getM0();
|
||||
item["delta-n"] = ae.second.getDeltan();
|
||||
item["omega-dot"] = ae.second.getOmegadot();
|
||||
item["omega0"] = ae.second.getOmega0();
|
||||
item["idot"] = ae.second.getIdot();
|
||||
item["t0e"] = ae.second.getT0e();
|
||||
|
||||
Point sat;
|
||||
getCoordinates(latestTow(2, svstats), ae.second, &sat);
|
||||
double E=getCoordinates(latestTow(2, svstats), ae.second, &sat);
|
||||
item["E"]=E;
|
||||
item["eph-ecefX"]= sat.x/1000;
|
||||
item["eph-ecefY"]= sat.y/1000;
|
||||
item["eph-ecefZ"]= sat.z/1000;
|
||||
|
|
@ -875,15 +902,34 @@ try
|
|||
for(const auto& src : g_srcpos) {
|
||||
nlohmann::json obj;
|
||||
obj["id"] = src.first;
|
||||
auto longlat = getLongLat(src.second.pos.x, src.second.pos.y, src.second.pos.z);
|
||||
longlat.first *= 180.0/M_PI;
|
||||
longlat.second *= 180.0/M_PI;
|
||||
longlat.first = ((int)(10*longlat.first))/10.0;
|
||||
longlat.second = ((int)(10*longlat.second))/10.0;
|
||||
obj["longitude"] = longlat.first;
|
||||
obj["latitude"] = longlat.second;
|
||||
auto latlonh = ecefToWGS84(src.second.pos.x, src.second.pos.y, src.second.pos.z);
|
||||
get<0>(latlonh) *= 180.0/M_PI;
|
||||
get<1>(latlonh) *= 180.0/M_PI;
|
||||
get<0>(latlonh) = ((int)(10*get<0>(latlonh)))/10.0;
|
||||
get<1>(latlonh) = ((int)(10*get<1>(latlonh)))/10.0;
|
||||
|
||||
get<2>(latlonh) = ((int)(10*get<2>(latlonh)))/10.0;
|
||||
|
||||
obj["latitude"] = get<0>(latlonh);
|
||||
obj["longitude"] = get<1>(latlonh);
|
||||
obj["last-seen"] = src.second.lastSeen;
|
||||
obj["ground-speed"] = src.second.groundSpeed;
|
||||
obj["swversion"] = src.second.swversion;
|
||||
obj["hwversion"] = src.second.hwversion;
|
||||
obj["mods"] = src.second.mods;
|
||||
obj["serialno"] = src.second.serialno;
|
||||
obj["clockoffsetns"]= src.second.clockOffsetNS;
|
||||
obj["clockdriftns"]= src.second.clockOffsetDriftNS;
|
||||
obj["clockacc"]= src.second.clockAccuracyNS;
|
||||
obj["freqacc"]= src.second.freqAccuracyPS;
|
||||
obj["uptime"]= src.second.uptime;
|
||||
obj["githash"]= src.second.githash;
|
||||
obj["owner"]= src.second.owner;
|
||||
obj["vendor"]= src.second.vendor;
|
||||
obj["remark"]= src.second.remark;
|
||||
|
||||
obj["acc"] = src.second.accuracy;
|
||||
obj["h"] = get<2>(latlonh);
|
||||
auto svstats = g_statskeeper.get();
|
||||
nlohmann::json svs = nlohmann::json::object();
|
||||
|
||||
|
|
@ -898,8 +944,14 @@ try
|
|||
|
||||
Point sat;
|
||||
|
||||
if((sv.first.gnss == 0 || sv.first.gnss == 2) && sv.second.completeIOD())
|
||||
if((sv.first.gnss == 0 || sv.first.gnss == 2) && sv.second.completeIOD()) {
|
||||
svo["delta_hz"] = iter->second.deltaHz;
|
||||
auto hzCorrection = getHzCorrection(time(0), src.first , sv.first.gnss, sv.first.sigid, svstats);
|
||||
if(hzCorrection)
|
||||
svo["delta_hz_corr"] = iter->second.deltaHz - *hzCorrection;
|
||||
|
||||
getCoordinates(latestTow(sv.first.gnss, svstats), sv.second.liveIOD(), & sat);
|
||||
}
|
||||
if(sv.first.gnss == 3 && sv.second.oldBeidouMessage.sow >= 0 && sv.second.oldBeidouMessage.sqrtA != 0) {
|
||||
getCoordinates(latestTow(sv.first.gnss, svstats), sv.second.oldBeidouMessage, &sat);
|
||||
}
|
||||
|
|
@ -914,8 +966,11 @@ try
|
|||
svo["azi"] = getAzimuthDeg(sat, our);
|
||||
}
|
||||
|
||||
|
||||
|
||||
svo["prres"] = iter->second.prres;
|
||||
svo["qi"] = iter->second.qi;
|
||||
svo["used"] = iter->second.used;
|
||||
svo["age-s"] = time(0) - iter->second.t;
|
||||
svo["last-seen"] = iter->second.t;
|
||||
svo["gnss"] = sv.first.gnss;
|
||||
|
|
@ -1064,6 +1119,21 @@ try
|
|||
ret["latitude"] = 180.0*longlat.second/M_PI;
|
||||
|
||||
}
|
||||
|
||||
nlohmann::json recvs = nlohmann::json::object();
|
||||
|
||||
for(const auto& perrecv : s.perrecv) {
|
||||
nlohmann::json recv = nlohmann::json::object();
|
||||
recv["db"] = perrecv.second.db;
|
||||
recv["qi"] = perrecv.second.qi;
|
||||
recv["used"] = perrecv.second.used;
|
||||
recv["prres"] = perrecv.second.prres;
|
||||
recv["elev"] = perrecv.second.el;
|
||||
recv["last-seen-s"] = time(0) - perrecv.second.t;
|
||||
recvs[std::to_string(perrecv.first)] = recv;
|
||||
|
||||
}
|
||||
ret["recvs"]=recvs;
|
||||
return ret;
|
||||
}
|
||||
);
|
||||
|
|
@ -1404,6 +1474,8 @@ try
|
|||
det["db"] = pr.second.db;
|
||||
det["last-seen-s"] = time(0) - pr.second.t;
|
||||
det["prres"] = pr.second.prres;
|
||||
det["qi"] = pr.second.qi;
|
||||
det["used"] = pr.second.used;
|
||||
|
||||
if(time(0) - pr.second.deltaHzTime < 60) {
|
||||
det["delta_hz"] = pr.second.deltaHz;
|
||||
|
|
@ -1484,11 +1556,24 @@ try
|
|||
sigid = 1;
|
||||
|
||||
SatID id{(uint32_t)gnssid, (uint32_t)sv, (uint32_t)sigid};
|
||||
g_svstats[id].perrecv[nmm.sourceid()].db = nmm.rd().db();
|
||||
g_svstats[id].perrecv[nmm.sourceid()].el = nmm.rd().el();
|
||||
g_svstats[id].perrecv[nmm.sourceid()].azi = nmm.rd().azi();
|
||||
g_svstats[id].perrecv[nmm.sourceid()].prres = nmm.rd().prres();
|
||||
auto& perrecv = g_svstats[id].perrecv[nmm.sourceid()];
|
||||
|
||||
perrecv.db = nmm.rd().db();
|
||||
|
||||
perrecv.el = nmm.rd().el();
|
||||
perrecv.azi = nmm.rd().azi();
|
||||
|
||||
perrecv.prres = nmm.rd().prres();
|
||||
if(nmm.rd().has_qi())
|
||||
perrecv.qi = nmm.rd().qi();
|
||||
else
|
||||
perrecv.qi = -1;
|
||||
|
||||
if(nmm.rd().has_used())
|
||||
perrecv.used = nmm.rd().used();
|
||||
else
|
||||
perrecv.used = -1;
|
||||
|
||||
Point sat{0,0,0};
|
||||
//cout<<"Got recdata for "<<id.gnss<<","<<id.sv<<","<<id.sigid<<": count="<<g_svstats.count(id)<<endl;
|
||||
// XXX this needs to be unified somehow
|
||||
|
|
@ -1498,13 +1583,18 @@ try
|
|||
else if(g_svstats[id].completeIOD()) {
|
||||
getCoordinates(g_svstats[id].tow, g_svstats[id].liveIOD(), &sat);
|
||||
}
|
||||
|
||||
if(sat.x != 0 && g_srcpos[nmm.sourceid()].pos.x != 0) {
|
||||
if(!(random() % 4))
|
||||
idb.addValue(id, nmm.localutcseconds()*1000000000, "recdata",
|
||||
{
|
||||
{"db", nmm.rd().db()},
|
||||
{"azi", getAzimuthDeg(sat, g_srcpos[nmm.sourceid()].pos)},
|
||||
{"ele", getElevationDeg(sat, g_srcpos[nmm.sourceid()].pos)},
|
||||
{"prres", nmm.rd().prres()}}, nmm.sourceid());
|
||||
{"prres", nmm.rd().prres()},
|
||||
{"qi", perrecv.qi},
|
||||
{"used", perrecv.used}
|
||||
}, nmm.sourceid());
|
||||
}
|
||||
|
||||
}
|
||||
|
|
@ -1527,12 +1617,13 @@ try
|
|||
|
||||
if(wtype == 1 || wtype == 2 || wtype == 3) {
|
||||
idb.addValue(id, "iod-live", svstat.galmsg.iodnav);
|
||||
idb.addValue(id, "eph-age", ephAge(gm.tow, gm.getT0e()));
|
||||
}
|
||||
if(wtype == 5 && svstat.galmsgTyped.count(5)) {
|
||||
const auto& old5gm = svstat.galmsgTyped[5];
|
||||
if(make_tuple(old5gm.e5bhs, old5gm.e1bhs, old5gm.e5bdvs, old5gm.e1bdvs) !=
|
||||
make_tuple(gm.e5bhs, gm.e1bhs, gm.e5bdvs, gm.e1bdvs)) {
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" Galileo "<<id.sv <<" sigid "<<id.sigid<<" change in health: ["<<humanBhs(old5gm.e5bhs)<<", "<<humanBhs(old5gm.e1bhs)<<", "<<(int)old5gm.e5bdvs <<", " << (int)old5gm.e1bdvs<<"] -> ["<< humanBhs(gm.e5bhs)<<", "<< humanBhs(gm.e1bhs)<<", "<< (int)gm.e5bdvs <<", " << (int)gm.e1bdvs<<"], lastseen "<<ephAge(old5gm.tow, gm.tow)/3600.0 <<" hours"<<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" src "<<nmm.sourceid()<<" Galileo "<<id.sv <<" sigid "<<id.sigid<<" change in health: ["<<humanBhs(old5gm.e5bhs)<<", "<<humanBhs(old5gm.e1bhs)<<", "<<(int)old5gm.e5bdvs <<", " << (int)old5gm.e1bdvs<<"] -> ["<< humanBhs(gm.e5bhs)<<", "<< humanBhs(gm.e1bhs)<<", "<< (int)gm.e5bdvs <<", " << (int)gm.e1bdvs<<"], lastseen "<<ephAge(old5gm.tow, gm.tow)/3600.0 <<" hours"<<endl;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1560,10 +1651,7 @@ try
|
|||
idb.addValue(id, "af2", g_svstats[id].iods[iod].af2);
|
||||
idb.addValue(id, "t0c", g_svstats[id].iods[iod].t0c * 60);
|
||||
|
||||
double age = ephAge(g_svstats[id].tow, g_svstats[id].iods[iod].t0c * 60);
|
||||
|
||||
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);
|
||||
idb.addValue(id, "atomic_offset_ns", svstat.galmsg.getAtomicOffset(svstat.tow).first);
|
||||
|
||||
if(oldgm.af0 && oldgm.t0c != svstat.galmsg.t0c) {
|
||||
auto oldOffset = oldgm.getAtomicOffset(svstat.tow);
|
||||
|
|
@ -1717,6 +1805,7 @@ try
|
|||
g_srcpos[nmm.sourceid()].groundSpeed = nmm.op().groundspeed();
|
||||
}
|
||||
|
||||
g_srcpos[nmm.sourceid()].accuracy = nmm.op().acc();
|
||||
// idb.addValueObserver(nmm.sourceid(), "accfix", nmm.op().acc(), nmm.localutcseconds());
|
||||
|
||||
auto latlonh = ecefToWGS84(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
|
|
@ -1747,12 +1836,20 @@ try
|
|||
{{"carrierphase", nmm.rfd().carrierphase()},
|
||||
{"doppler", nmm.rfd().doppler()},
|
||||
{"locktime", nmm.rfd().locktimems()},
|
||||
{"pseudorange", nmm.rfd().pseudorange()}});
|
||||
{"pseudorange", nmm.rfd().pseudorange()},
|
||||
{"prstd", nmm.rfd().prstd()},
|
||||
{"cpstd", nmm.rfd().cpstd()},
|
||||
{"dostd", nmm.rfd().dostd()}
|
||||
|
||||
}, nmm.sourceid());
|
||||
if(id.gnss == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
|
||||
double freq = 1561.098;
|
||||
if(nmm.rfd().sigid() != 0)
|
||||
freq = 1207.140;
|
||||
auto res = doDoppler(nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()].pos, g_svstats[id].oldBeidouMessage, freq * 1000000);
|
||||
|
||||
// the magic 14 is because 'rcvtow()' is in GPS/Galileo TOW
|
||||
// but BeiDou operates with 14 leap seconds less than GPS/Galileo
|
||||
auto res = doDoppler(nmm.rfd().rcvtow()-14, g_srcpos[nmm.sourceid()].pos, g_svstats[id].oldBeidouMessage, freq * 1000000);
|
||||
|
||||
if(isnan(res.preddop)) {
|
||||
cerr<<"Problem with doppler calculation for C"<<id.sv<<": "<<endl;
|
||||
|
|
@ -1768,7 +1865,19 @@ try
|
|||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
||||
auto corr = getHzCorrection(t, nmm.sourceid(), id.gnss, id.sigid, g_svstats);
|
||||
if(corr) {
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - (1561.098/1575.42) * (*corr));
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - (*corr));
|
||||
Point sat;
|
||||
getCoordinates(nmm.rfd().rcvtow(), g_svstats[id].oldBeidouMessage, &sat);
|
||||
|
||||
idb.addValue(id, 1000000000.0*t, "correlator",
|
||||
{{"delta_hz_cor", nmm.rfd().doppler() - res.preddop - (*corr)},
|
||||
{"delta_hz", nmm.rfd().doppler() - res.preddop},
|
||||
{"elevation", getElevationDeg(sat, g_srcpos[nmm.sourceid()].pos)},
|
||||
{"prres", g_svstats[id].perrecv[nmm.sourceid()].prres},
|
||||
{"qi", g_svstats[id].perrecv[nmm.sourceid()].qi}
|
||||
}, nmm.sourceid());
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -1779,25 +1888,135 @@ try
|
|||
|
||||
auto res = doDoppler(nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()].pos, g_svstats[id].liveIOD(),freqMHZ * 1000000);
|
||||
|
||||
idb.addValue(id, nanoTime(0, nmm.rfd().rcvwn(), nmm.rfd().rcvtow()), "orbit",
|
||||
{{"preddop", res.preddop},
|
||||
{"radvel", res.radvel}});
|
||||
|
||||
|
||||
Point sat;
|
||||
getCoordinates(nmm.rfd().rcvtow(), g_svstats[id].liveIOD(), &sat);
|
||||
|
||||
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
||||
// idb.addValueObserver((int)nmm.sourceid(), "orbit",
|
||||
// {{"preddop", res.preddop},
|
||||
// {"radvel", res.radvel}}, t, id);
|
||||
|
||||
if(t - g_svstats[id].perrecv[nmm.sourceid()].deltaHzTime > 10) { // only replace after 10 seconds
|
||||
|
||||
|
||||
if(t - g_svstats[id].perrecv[nmm.sourceid()].deltaHzTime > 10) { // only replace after 5 seconds
|
||||
g_svstats[id].perrecv[nmm.sourceid()].deltaHz = nmm.rfd().doppler() - res.preddop;
|
||||
g_svstats[id].perrecv[nmm.sourceid()].deltaHzTime = t;
|
||||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
||||
auto corr = getHzCorrection(t, nmm.sourceid(), id.gnss, id.sigid, g_svstats);
|
||||
if(corr) {
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - *corr);
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - *corr, nmm.sourceid());
|
||||
idb.addValue(id, 1000000000.0*t, "correlator",
|
||||
{{"delta_hz_cor", nmm.rfd().doppler() - res.preddop - (*corr)},
|
||||
{"delta_hz", nmm.rfd().doppler() - res.preddop},
|
||||
{"hz", nmm.rfd().doppler()},
|
||||
{"elevation", getElevationDeg(sat, g_srcpos[nmm.sourceid()].pos)},
|
||||
{"prres", g_svstats[id].perrecv[nmm.sourceid()].prres},
|
||||
{"qi", g_svstats[id].perrecv[nmm.sourceid()].qi}
|
||||
}, nmm.sourceid());
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::ObserverDetailsType) {
|
||||
auto& o = g_srcpos[nmm.sourceid()];
|
||||
o.serialno = nmm.od().serialno();
|
||||
|
||||
o.swversion = nmm.od().swversion();
|
||||
o.hwversion = nmm.od().hwversion();
|
||||
o.mods = nmm.od().modules();
|
||||
if(nmm.od().has_clockoffsetns())
|
||||
o.clockOffsetNS = nmm.od().clockoffsetns();
|
||||
else
|
||||
o.clockOffsetNS = -1;
|
||||
|
||||
if(nmm.od().has_clockoffsetdriftns())
|
||||
o.clockOffsetDriftNS = nmm.od().clockoffsetdriftns();
|
||||
else
|
||||
o.clockOffsetDriftNS = -1;
|
||||
|
||||
if(nmm.od().has_clockaccuracyns())
|
||||
o.clockAccuracyNS = nmm.od().clockaccuracyns();
|
||||
else
|
||||
o.clockAccuracyNS = -1;
|
||||
|
||||
if(nmm.od().has_freqaccuracyps())
|
||||
o.freqAccuracyPS = nmm.od().freqaccuracyps();
|
||||
else
|
||||
o.freqAccuracyPS = -1;
|
||||
|
||||
if(nmm.od().has_uptime())
|
||||
o.uptime = nmm.od().uptime();
|
||||
else
|
||||
o.uptime = -1;
|
||||
|
||||
if(nmm.od().has_recvgithash())
|
||||
o.githash = nmm.od().recvgithash();
|
||||
else
|
||||
o.githash.clear();
|
||||
|
||||
|
||||
o.vendor = nmm.od().vendor();
|
||||
o.owner = nmm.od().owner();
|
||||
o.remark = nmm.od().remark();
|
||||
|
||||
|
||||
idb.addValueObserver(nmm.sourceid(), "observer_details", {
|
||||
{"clock_offset_ns", o.clockOffsetNS},
|
||||
{"clock_drift_ns", o.clockOffsetDriftNS},
|
||||
{"clock_acc_ns", o.clockAccuracyNS},
|
||||
{"freq_acc_ps", o.freqAccuracyPS},
|
||||
{"uptime", o.uptime}
|
||||
},
|
||||
nmm.localutcseconds());
|
||||
|
||||
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::DebuggingType) {
|
||||
auto ret = parseTrkMeas(basic_string<uint8_t>((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
for(const auto& tss : ret) {
|
||||
SatID id{tss.gnss, tss.sv, tss.gnss == 2 ? 1 : 0};
|
||||
if(g_svstats[id].completeIOD()) {
|
||||
double freqMHZ = 1575.42;
|
||||
double tsat = ldexp(1.0* tss.tr, -32) /1000.0;
|
||||
auto res = doDoppler(tsat, g_srcpos[nmm.sourceid()].pos, g_svstats[id].liveIOD(),freqMHZ * 1000000);
|
||||
|
||||
|
||||
idb.addValueObserver((int)nmm.sourceid(), "orbit",
|
||||
{{"preddop", res.preddop},
|
||||
{"radvel", res.radvel}}, (time_t)nmm.localutcseconds(), id);
|
||||
|
||||
// cout << " preddop "<<res.preddop <<" meas "<<tss.dopplerHz <<" delta " << (tss.dopplerHz - res.preddop);
|
||||
double t = utcFromGPS(latestWN(0, g_svstats), tsat);
|
||||
if(t - g_svstats[id].perrecv[nmm.sourceid()].deltaHzTime > 5) { // only replace after 5 seconds
|
||||
g_svstats[id].perrecv[nmm.sourceid()].deltaHz = tss.dopplerHz - res.preddop;
|
||||
g_svstats[id].perrecv[nmm.sourceid()].deltaHzTime = t;
|
||||
idb.addValue(id, "delta_hz", tss.dopplerHz - res.preddop);
|
||||
|
||||
auto corr = getHzCorrection(t, nmm.sourceid(), id.gnss, id.sigid, g_svstats);
|
||||
if(corr) {
|
||||
Point sat;
|
||||
getCoordinates(tsat, g_svstats[id].liveIOD(), &sat);
|
||||
|
||||
idb.addValue(id, "delta_hz_cor", tss.dopplerHz - res.preddop - *corr, nmm.sourceid());
|
||||
|
||||
idb.addValue(id, 1000000000.0*t, "correlator",
|
||||
{{"delta_hz_cor", tss.dopplerHz - res.preddop - *corr},
|
||||
{"delta_hz", tss.dopplerHz - res.preddop},
|
||||
{"hz", tss.dopplerHz},
|
||||
{"elevation", getElevationDeg(sat, g_srcpos[nmm.sourceid()].pos)},
|
||||
{"prres", g_svstats[id].perrecv[nmm.sourceid()].prres},
|
||||
{"qi", g_svstats[id].perrecv[nmm.sourceid()].qi}
|
||||
}, nmm.sourceid());
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
// cout<<endl;
|
||||
}
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::GPSInavType) {
|
||||
if(nmm.gpsi().sigid()) {
|
||||
cout<<"ignoring sigid "<<nmm.gpsi().sigid()<<" for GPS "<<nmm.gpsi().gnsssv()<<endl;
|
||||
|
|
@ -1821,11 +2040,11 @@ try
|
|||
idb.addValue(id, "ura", svstat.ura);
|
||||
|
||||
if(oldsvstat.af0 && oldsvstat.ura != svstat.ura) { // XX find better way to check
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" wn "<<svstat.wn <<" GPS "<<id.sv <<"@"<<id.sigid<<" change in URA ["<< humanUra(oldsvstat.ura) <<"] -> [" << humanUra(svstat.ura)<<"] "<<(int)svstat.ura<<", lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" src "<<nmm.sourceid()<<" wn "<<svstat.wn <<" GPS "<<id.sv <<"@"<<id.sigid<<" change in URA ["<< humanUra(oldsvstat.ura) <<"] -> [" << humanUra(svstat.ura)<<"] "<<(int)svstat.ura<<", lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
}
|
||||
|
||||
if(oldsvstat.af0 && oldsvstat.gpshealth != svstat.gpshealth) { // XX find better way to check
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" wn "<<svstat.wn <<" GPS "<<id.sv <<"@"<<id.sigid<<" change in health ["<< (int)oldsvstat.gpshealth <<"] -> [" << (int)svstat.gpshealth <<"], lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" src " <<nmm.sourceid()<<" wn "<<svstat.wn <<" GPS "<<id.sv <<"@"<<id.sigid<<" change in health ["<< (int)oldsvstat.gpshealth <<"] -> [" << (int)svstat.gpshealth <<"], lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1843,7 +2062,7 @@ try
|
|||
}
|
||||
else if(frame==2) {
|
||||
if(oldsvstat.gpshealth != svstat.gpshealth) {
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" GPS "<<id.sv <<"@"<<id.sigid<<" change in health: ["<< (int)oldsvstat.gpshealth<<"] -> ["<< (int)svstat.gpshealth<<"] , lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" src "<<nmm.sourceid()<<" GPS "<<id.sv <<"@"<<id.sigid<<" change in health: ["<< (int)oldsvstat.gpshealth<<"] -> ["<< (int)svstat.gpshealth<<"] , lastseen "<<ephAge(oldsvstat.tow, svstat.tow)/3600.0 <<" hours"<<endl;
|
||||
}
|
||||
idb.addValue(id, "gpshealth", g_svstats[id].gpshealth);
|
||||
}
|
||||
|
|
@ -1866,6 +2085,7 @@ try
|
|||
g_svstats[id].wn += 2048;
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD1) {
|
||||
try {
|
||||
SatID id{nmm.bid1().gnssid(), nmm.bid1().gnsssv(), nmm.bid1().sigid()};
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
|
@ -1977,6 +2197,9 @@ try
|
|||
g_dtLSBeidou = bm.deltaTLS;
|
||||
// cout<<"Beidou leap seconds: "<<g_dtLSBeidou<<endl;
|
||||
}
|
||||
}catch(std::exception& e) {
|
||||
cerr<<"Beidou: "<<e.what()<<endl;
|
||||
}
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD2) {
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
|
|
@ -2007,6 +2230,7 @@ try
|
|||
// cout<<"Glonass now: "<<humanTime(glotime + 820368000) << " n4 "<<(int)gm.n4<<" NT "<<(int)gm.NT<< " wn "<<svstat.wn <<" tow " <<svstat.tow<<endl;
|
||||
}
|
||||
else if(strno == 2) {
|
||||
idb.addValue(id, "glohealth", gm.Bn);
|
||||
if(oldgm.Bn != gm.Bn) {
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" n4 "<< (int)gm.n4<<" NT " << (int)gm.NT<<" GLONASS R"<<id.sv<<"@"<<id.sigid<<" health changed from "<<(int)oldgm.Bn <<" to "<< (int)gm.Bn <<endl;
|
||||
|
||||
|
|
|
|||
|
|
@ -40,7 +40,7 @@ int main(int argc, char **argv)
|
|||
{
|
||||
MiniCurl mc;
|
||||
MiniCurl::MiniCurlHeaders mch;
|
||||
string dbname("galileo3");
|
||||
string dbname("galileo");
|
||||
|
||||
string url="http://127.0.0.1:8086/query?db="+dbname+"&epoch=s&q=";
|
||||
string period="time > now() - 1w";
|
||||
|
|
|
|||
|
|
@ -0,0 +1,125 @@
|
|||
#include <string>
|
||||
#include "fmt/format.h"
|
||||
#include "fmt/printf.h"
|
||||
#include <vector>
|
||||
#include <map>
|
||||
#include <iostream>
|
||||
#include "ubx.hh"
|
||||
using namespace std;
|
||||
/* TRK-MEAS
|
||||
|
||||
2 U1 Number of channels tracked
|
||||
?
|
||||
8 U4 Poweron counter milliseconds
|
||||
12 U4 "session time counter" milliseconds
|
||||
|
||||
104 + n*56 + 0 U1 Channel (?)
|
||||
104 + n*56 + 1 U1 Quality indicator 0 idle, 1 search, 2 acquired, 3 unsusable, 4 code lock, 5/6/7 carrier lock)
|
||||
+ 2 ?
|
||||
+ 3 ?
|
||||
104 + n*56 + 4 U1 GNSS (?)
|
||||
104 + n*56 + 5 U1 Satellite ID
|
||||
?
|
||||
104 + n*56 + 7 U1 GLONASS Frequency
|
||||
104 + n*56 + 8 U1 Tracking status
|
||||
?
|
||||
+ 16 U1 Code lock "count"
|
||||
+ 17 U1 Phase lock "count"
|
||||
?
|
||||
+ 20 U2 256*dB
|
||||
?
|
||||
104 + n*56 + 24 I8 Transmission time, in units of 2^-32 milliseconds (?) // ts=I8(p+24)*P2_32/1000.0;
|
||||
|
||||
+ 32 I8 "ADR" in units of 2^-32, plus 0.5 if 'bit' 0x40 is set in tracking status
|
||||
+ 40 I4 Doppler in units of 2^-10 deca-hertz (?) // I4(p+40)*P2_10*10.0;
|
||||
+ 44 ?
|
||||
..
|
||||
+ 56
|
||||
*/
|
||||
|
||||
extern "C" {
|
||||
int ubxdecrypt(const unsigned char crypto[16], unsigned char plaintext[16]) __attribute__((weak));
|
||||
int ubxdecrypt(const unsigned char crypto[16], unsigned char plaintext[16])
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
vector<TrkSatStat> parseTrkMeas(std::basic_string_view<uint8_t> payload)
|
||||
{
|
||||
uint8_t plainchunk[16];
|
||||
std::basic_string<uint8_t> plaintext;
|
||||
/*
|
||||
cerr<<"payload.size(): "<<payload.size()<<", mod "<<(payload.size()-4)%16<<", " << payload[2]+256*payload[3]<<": ";
|
||||
for(int n=0; n < 4; ++n)
|
||||
cerr<<fmt::sprintf("%02x ", (unsigned int) payload[n]);
|
||||
cerr<<endl;
|
||||
*/
|
||||
int msgsize = (payload[2]+256*payload[3]) - 6;
|
||||
|
||||
vector<TrkSatStat> ret;
|
||||
for(unsigned int n=4 ; n < payload.size(); n+=16) {
|
||||
if(ubxdecrypt(&payload[n], plainchunk) < 0)
|
||||
return ret;
|
||||
plaintext.append(plainchunk, plainchunk+16);
|
||||
}
|
||||
// cerr<<"msgsize: "<<msgsize<<", assumed SVs: "<<(int)plaintext[8]<<", per "<<(msgsize-104.0)/(int)plaintext[8]<<endl;
|
||||
|
||||
int64_t maxtr=0;
|
||||
for(int n = 0 ; n < plaintext[8]; ++n) {
|
||||
int offset = 110+n*56;
|
||||
int64_t tr;
|
||||
memcpy(&tr, &plaintext[offset+24], 8);
|
||||
if(tr > maxtr)
|
||||
maxtr = tr;
|
||||
}
|
||||
|
||||
|
||||
|
||||
for(int n = 0 ; n < plaintext[8]; ++n) {
|
||||
int offset = 110+n*56;
|
||||
int32_t rdoppler;
|
||||
int64_t tr;
|
||||
memcpy(&rdoppler, &plaintext[offset+40], 4);
|
||||
|
||||
memcpy(&tr, &plaintext[offset+24], 8);
|
||||
|
||||
int gnssid = plaintext[offset+4];
|
||||
int sv = plaintext[offset+5];
|
||||
int trkstat = plaintext[offset+8];
|
||||
double doppler = ldexp(1.0*rdoppler,-10)*10;
|
||||
int plcount = (unsigned int)plaintext[offset+17];
|
||||
/*
|
||||
cerr<<" gnssid " << gnssid <<" sv "<< sv;
|
||||
cerr<<" qi "<<(unsigned int)plaintext[offset+1] <<" c "<<(unsigned int)plaintext[offset];
|
||||
cerr<<" ? "<<(unsigned int)plaintext[offset+2] << " ? " <<(unsigned int)plaintext[offset+3];
|
||||
cerr<<" trkstat " << trkstat<<" db " << (unsigned int) plaintext[offset+21] << " doppler "<<
|
||||
doppler << "Hz tr " << tr<< " cl-count " << (unsigned int)plaintext[offset+16]<< " pl-count " << plcount;
|
||||
cerr<<" tau " << ldexp((tr - maxtr), -32)/1000.0;
|
||||
cerr<<" pr " << 3e5*ldexp((tr - maxtr), -32)/1000.0;
|
||||
cerr<<endl;
|
||||
*/
|
||||
if((gnssid== 0 || gnssid == 2) && plcount > 2) {
|
||||
struct TrkSatStat tss;
|
||||
tss.sv = sv;
|
||||
tss.gnss = gnssid;
|
||||
tss.dopplerHz = doppler;
|
||||
tss.tr = tr;
|
||||
ret.push_back(tss);
|
||||
}
|
||||
|
||||
}
|
||||
/*
|
||||
for(int n = 0 ; n < plaintext[8]; ++n) {
|
||||
int offset = 110+n*56;
|
||||
cerr<<" gnssid " << (unsigned int)plaintext[offset+4]<<" sv "<<(unsigned int) plaintext[offset+5]<<endl;
|
||||
for(int j = 0; j < 56; ++j) {
|
||||
cerr<< fmt::sprintf("%02x ", (unsigned int)plaintext[offset+j]);
|
||||
if((j % 16)==15)
|
||||
cerr<<endl;
|
||||
}
|
||||
cerr<<endl;
|
||||
}
|
||||
*/
|
||||
return ret;
|
||||
}
|
||||
11
ubx.hh
11
ubx.hh
|
|
@ -1,3 +1,4 @@
|
|||
#pragma once
|
||||
#include <string>
|
||||
#include <vector>
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str);
|
||||
|
|
@ -11,3 +12,13 @@ std::basic_string<uint8_t> getGlonassFromSFRBXMsg(std::basic_string_view<uint8_t
|
|||
std::basic_string<uint8_t> getBeidouFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
std::basic_string<uint8_t> getSBASFromSFRBXMsg(std::basic_string_view<uint8_t> msg);
|
||||
struct CRCMismatch{};
|
||||
|
||||
struct TrkSatStat
|
||||
{
|
||||
int gnss;
|
||||
int sv;
|
||||
double dopplerHz;
|
||||
uint64_t tr;
|
||||
};
|
||||
|
||||
std::vector<TrkSatStat> parseTrkMeas(std::basic_string_view<uint8_t> payload);
|
||||
|
|
|
|||
17
ubxtool.cc
17
ubxtool.cc
|
|
@ -756,6 +756,23 @@ int main(int argc, char** argv)
|
|||
cerr<<humanTimeNow()<<" Got nack on F9P GNSS setting"<<endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
/* VALSET
|
||||
0x20 91 02 32 =
|
||||
*/
|
||||
|
||||
msg = buildUbxMessage(0x06, 0x8a, {0x00, 0x01, 0x00, 0x00,
|
||||
0x07, 0x00, 0x91, 0x20, 1,
|
||||
0x32, 0x02, 0x91, 0x20, 1});
|
||||
if(sendAndWaitForUBXAckNack(fd, 2, msg, 0x06, 0x8a)) { // msg cfg F9P
|
||||
if (doDEBUG) { cerr<<humanTimeNow()<<" Got ack on F9P UART1 setting"<<endl; }
|
||||
}
|
||||
else {
|
||||
cerr<<humanTimeNow()<<" Got nack on F9P UART1 setting"<<endl;
|
||||
exit(-1);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
if(doSBAS) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue