just too much progess
bert hubert
parent
f4e2235979
commit
406b440ae1
44
ephemeris.cc
44
ephemeris.cc
|
|
@ -48,3 +48,47 @@ std::pair<double,double> getLongLat(double x, double y, double z)
|
|||
|
||||
return std::make_pair(longitude, latitude);
|
||||
}
|
||||
|
||||
|
||||
double getElevationDeg(const Point& sat, const Point& our)
|
||||
{
|
||||
|
||||
Point core{0,0,0};
|
||||
|
||||
Vector core2us(core, our);
|
||||
Vector dx(our, sat); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
||||
// https://ds9a.nl/articles/
|
||||
|
||||
double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
|
||||
double deg = 180.0* (elev/M_PI);
|
||||
return 90.0 - deg;
|
||||
}
|
||||
|
||||
// https://gis.stackexchange.com/questions/58923/calculating-view-angle
|
||||
|
||||
double getAzimuthDeg(const Point& sat, const Point& our)
|
||||
{
|
||||
Point core{0,0,0};
|
||||
|
||||
Vector north{
|
||||
-our.z*our.x,
|
||||
-our.z*our.y,
|
||||
our.x*our.x + our.y * our.y};
|
||||
|
||||
Vector east{-our.y, our.x, 0};
|
||||
|
||||
Vector dx(our, sat); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
||||
// https://ds9a.nl/articles/
|
||||
|
||||
double azicos = ( north.inner(dx) / (north.length() * dx.length()));
|
||||
double azisin = ( east.inner(dx) / (east.length() * dx.length()));
|
||||
|
||||
double azi = atan2(azisin, azicos);
|
||||
|
||||
double deg = 180.0* (azi/M_PI);
|
||||
if(deg < 0)
|
||||
deg += 360;
|
||||
return deg;
|
||||
}
|
||||
|
|
|
|||
16
ephemeris.hh
16
ephemeris.hh
|
|
@ -5,7 +5,7 @@
|
|||
int ephAge(int tow, int t0e);
|
||||
|
||||
template<typename T>
|
||||
void getCoordinates(int wn, double tow, const T& iod, Point* p, bool quiet=true)
|
||||
void getCoordinates(double tow, const T& iod, Point* p, bool quiet=true)
|
||||
{
|
||||
using namespace std;
|
||||
// here goes
|
||||
|
|
@ -165,25 +165,25 @@ struct DopplerData
|
|||
};
|
||||
|
||||
template<typename T>
|
||||
void getSpeed(int wn, double tow, const T& eph, Vector* v)
|
||||
void getSpeed(double tow, const T& eph, Vector* v)
|
||||
{
|
||||
Point a, b;
|
||||
getCoordinates(wn, tow-0.5, eph, &a);
|
||||
getCoordinates(wn, tow+0.5, eph, &b);
|
||||
getCoordinates(tow-0.5, eph, &a);
|
||||
getCoordinates(tow+0.5, eph, &b);
|
||||
*v = Vector(a, b);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
DopplerData doDoppler(int wn, int tow, const Point& us, const T& eph, double freq)
|
||||
DopplerData doDoppler(double tow, const Point& us, const T& eph, double freq)
|
||||
{
|
||||
DopplerData ret;
|
||||
|
||||
// be careful with time here - we need to evaluate at the timestamp of this RFDataType update
|
||||
// which might be newer than .tow in g_svstats
|
||||
getCoordinates(wn, tow, eph, &ret.sat);
|
||||
getCoordinates(tow, eph, &ret.sat);
|
||||
Point core;
|
||||
Vector us2sat(us, ret.sat);
|
||||
getSpeed(wn, tow, eph, &ret.speed);
|
||||
getSpeed(tow, eph, &ret.speed);
|
||||
Vector core2us(core, us);
|
||||
Vector dx(us, ret.sat); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
||||
|
|
@ -201,3 +201,5 @@ DopplerData doDoppler(int wn, int tow, const Point& us, const T& eph, double fre
|
|||
}
|
||||
|
||||
std::pair<double,double> getLongLat(double x, double y, double z);
|
||||
double getElevationDeg(const Point& sat, const Point& our);
|
||||
double getAzimuthDeg(const Point& sat, const Point& our);
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ function maketable(str, arr)
|
|||
enter().
|
||||
append("tr");
|
||||
|
||||
var columns = ["sv", "best-tle", "best-tle-dist", "best-tle-norad", "best-tle-int-desig", "e1bhs", "e5bhs", "health", "inclination", "eph-ecefX", "eph-ecefY", "eph-ecefZ", "tle-ecefX", "tle-ecefY", "tle-ecefZ", "eph-latitude", "eph-longitude", "tle-latitude", "tle-longitude", "t0e", "t"]; // , "tle-eciX", "tle-eciY", "tle-eciZ"
|
||||
var columns = ["sv", "best-tle", "best-tle-dist", "best-tle-norad", "best-tle-int-desig", "eph-ecefX", "eph-ecefY", "eph-ecefZ", "tle-ecefX", "tle-ecefY", "tle-ecefZ", "eph-latitude", "eph-longitude", "tle-latitude", "tle-longitude", "tle-eciX", "tle-eciY", "tle-eciZ", "t0e", "t"];
|
||||
|
||||
// append the header row
|
||||
thead.append("tr")
|
||||
|
|
@ -23,10 +23,8 @@ function maketable(str, arr)
|
|||
.enter()
|
||||
.append("th")
|
||||
.text(function(column) {
|
||||
if(column == "best-tle-dist")
|
||||
return "tle-dist";
|
||||
if(column == "best-tle-int-desig")
|
||||
return "int-desig";
|
||||
if(column == "delta_hz_corr")
|
||||
return "ΔHz";
|
||||
else
|
||||
return column;
|
||||
});
|
||||
|
|
@ -37,9 +35,7 @@ function maketable(str, arr)
|
|||
var ret={};
|
||||
ret.column = column;
|
||||
ret.color=null;
|
||||
if(row[column] != null && column != "sv" && column != "best-tle" &&
|
||||
column != "best-tle-norad" && column != "best-tle-int-desig" && column != "e1bhs" &&
|
||||
column != "e5bhs" && column!="health")
|
||||
if(row[column] != null && column != "sv" && column != "best-tle" && column != "best-tle-norad" && column != "best-tle-int-desig")
|
||||
ret.value = row[column].toFixed(1);
|
||||
else
|
||||
ret.value = row[column];
|
||||
|
|
@ -90,6 +86,9 @@ function update()
|
|||
var livearr=[];
|
||||
for(n = 0 ; n < arr.length; n++)
|
||||
{
|
||||
if(arr[n].sv[0]!='G')
|
||||
continue;
|
||||
|
||||
livearr.push(arr[n]);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -200,6 +200,7 @@ function update()
|
|||
var livearr=[], stalearr=[];
|
||||
for(n = 0 ; n < arr.length; n++)
|
||||
{
|
||||
// if(arr[n]["gnssid"]) continue;
|
||||
if(arr[n]["last-seen-s"] < 600)
|
||||
livearr.push(arr[n]);
|
||||
else
|
||||
|
|
@ -212,6 +213,6 @@ function update()
|
|||
});
|
||||
}
|
||||
|
||||
update();
|
||||
repeat=update();
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -80,7 +80,8 @@ function update()
|
|||
.attr("cx", d => aProjection([d["eph-longitude"],d["eph-latitude"]])[0])
|
||||
.attr("cy", d => aProjection([d["eph-longitude"],d["eph-latitude"]])[1])
|
||||
.attr("fill", function(d) { if(d.gnssid==2) return "blue";
|
||||
if(d.gnssid==3) return "red";
|
||||
else if(d.gnssid==3) return "red";
|
||||
else if(d.gnssid==6) return "yellow";
|
||||
else return "green"; });
|
||||
|
||||
svg.selectAll("text").data(arr)
|
||||
|
|
@ -88,7 +89,9 @@ function update()
|
|||
.append("text")
|
||||
.attr("dx", d => 5+aProjection([d["eph-longitude"],d["eph-latitude"]])[0])
|
||||
.attr("dy", d => 5+aProjection([d["eph-longitude"],d["eph-latitude"]])[1])
|
||||
.text(d => d.sv);
|
||||
.text(d => d.sv)
|
||||
.attr("fill", function(d) { if(d.observed==true) return "black"; return "#666666"; })
|
||||
.attr("font-weight", function(d) { if(d.observed==true) return "bold"; return null; });
|
||||
|
||||
svg.selectAll("rect").data(results[1])
|
||||
.enter()
|
||||
|
|
|
|||
36
navdump.cc
36
navdump.cc
|
|
@ -26,6 +26,8 @@
|
|||
#include <unistd.h>
|
||||
using namespace std;
|
||||
|
||||
Point g_ourpos;
|
||||
|
||||
string beidouHealth(int in)
|
||||
{
|
||||
string ret;
|
||||
|
|
@ -108,7 +110,7 @@ try
|
|||
cout<<humanTime(nmm.localutcseconds())<<" "<<nmm.localutcnanoseconds()<<" ";
|
||||
cout<<"src "<<nmm.sourceid()<< " ";
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<", db "<<nmm.rd().db()<<" ele "<<nmm.rd().el() <<" azi "<<nmm.rd().azi()<<" prRes "<<nmm.rd().prres() << endl;
|
||||
cout<<"receptiondata for "<<nmm.rd().gnssid()<<","<<nmm.rd().gnsssv()<<","<< (nmm.rd().has_sigid() ? nmm.rd().sigid() : 0) <<" db "<<nmm.rd().db()<<" ele "<<nmm.rd().el() <<" azi "<<nmm.rd().azi()<<" prRes "<<nmm.rd().prres() << endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
if(skipGalileo)
|
||||
|
|
@ -123,7 +125,7 @@ try
|
|||
|
||||
gm.tow = nmm.gi().gnsstow();
|
||||
gmwtypes[{nmm.gi().gnsssv(), wtype}] = gm;
|
||||
cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype<<" ";
|
||||
cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<","<<nmm.gi().sigid()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype<<" ";
|
||||
static uint32_t tow;
|
||||
if(wtype == 4) {
|
||||
// 2^-34 2^-46
|
||||
|
|
@ -158,13 +160,14 @@ try
|
|||
cout<<" t0a "<<gm.t0almanac<<", alma sv1 "<<gm.alma1.svid<<", t0a age: "<< ephAge(gm.t0almanac *600, tow) << " ";
|
||||
if(gm.alma1.svid) {
|
||||
Point satpos;
|
||||
getCoordinates(0, gm.tow, gm.alma1, &satpos);
|
||||
getCoordinates(gm.tow, gm.alma1, &satpos);
|
||||
cout<< "("<<satpos.x/1000<<", "<<satpos.y/1000<<", "<<satpos.z/1000<<")";
|
||||
|
||||
auto match = tles.getBestMatch(nmm.localutcseconds(), satpos.x, satpos.y, satpos.z);
|
||||
cout<<" best-tle-match "<<match.name <<" distance "<<match.distance /1000<<" km ";
|
||||
cout <<" tle-e "<<match.e <<" eph-e " <<gm.alma1.getE() <<" tle-ran "<<match.ran;
|
||||
cout<<" norad " <<match.norad <<" int-desig " << match.internat;
|
||||
cout<<" ele " << getElevationDeg(satpos, g_ourpos) << " azi " << getAzimuthDeg(satpos, g_ourpos);
|
||||
}
|
||||
}
|
||||
else if(wtype == 8 && gm.tow - gmwtypes[{sv,7}].tow < 5 && gmwtypes[{sv,7}].alma1.svid && gm.iodalmanac == gmwtypes[{sv,7}].iodalmanac) {
|
||||
|
|
@ -197,7 +200,7 @@ try
|
|||
uint8_t page;
|
||||
static int gpswn;
|
||||
int frame=parseGPSMessage(cond, gs, &page);
|
||||
cout<<"GPS "<<sv<<": "<<gs.tow<<" frame "<<frame<<" ";
|
||||
cout<<"GPS "<<sv<<"@"<<nmm.gpsi().sigid()<<": "<<gs.tow<<" frame "<<frame<<" ";
|
||||
if(frame == 1) {
|
||||
static map<int, GPSState> oldgs1s;
|
||||
gpswn = gs.wn;
|
||||
|
|
@ -218,20 +221,21 @@ try
|
|||
cout <<"iod "<<gs.gpsiod;
|
||||
if(eph[sv].isComplete(gs.gpsiod)) {
|
||||
Point sat;
|
||||
getCoordinates(0, gs.tow, eph[sv].iods[gs.gpsiod], &sat);
|
||||
getCoordinates(gs.tow, eph[sv].iods[gs.gpsiod], &sat);
|
||||
TLERepo::Match second;
|
||||
auto match = tles.getBestMatch(utcFromGPS(gpswn, gs.tow), sat.x, sat.y, sat.z, &second);
|
||||
cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
|
||||
cout<<" norad " <<match.norad <<" int-desig " << match.internat;
|
||||
cout<<" 2nd-match "<<second.name << " dist "<<second.distance/1000<<" km t0e "<<gs.gpsalma.getT0e() << " t " <<nmm.localutcseconds();
|
||||
cout<<" ele " << getElevationDeg(sat, g_ourpos) << " azi " << getAzimuthDeg(sat, g_ourpos);
|
||||
|
||||
if(almas.count(sv)) {
|
||||
Point almapoint;
|
||||
getCoordinates(0, gs.tow, almas[sv], &almapoint);
|
||||
getCoordinates(gs.tow, almas[sv], &almapoint);
|
||||
cout<<" alma-dist " << Vector(sat, almapoint).length();
|
||||
|
||||
Vector speed;
|
||||
getSpeed(0, gs.tow, eph[sv].iods[gs.gpsiod], &speed);
|
||||
getSpeed(gs.tow, eph[sv].iods[gs.gpsiod], &speed);
|
||||
Point core;
|
||||
csv << nmm.localutcseconds() << " 0 "<< sv <<" " << gs.tow << " " << match.distance <<" " << Vector(sat, almapoint).length() << " " << utcFromGPS(gpswn, gs.tow) - nmm.localutcseconds() << " " << sat.x <<" " << sat.y <<" " << sat.z <<" " <<speed.x <<" " <<speed.y<<" " <<speed.z<< " " << Vector(core, sat).length() << " " << eph[sv].iods[gs.gpsiod].getI0()<<" " << eph[sv].iods[gs.gpsiod].getE() << " " <<gs.gpsiod<<endl;
|
||||
}
|
||||
|
|
@ -243,7 +247,7 @@ try
|
|||
if((gs.gpsalma.sv >= 25 && gs.gpsalma.sv <= 32) || gs.gpsalma.sv==57 ) { // see table 20-V of the GPS ICD
|
||||
cout << " data-id "<<gs.gpsalma.dataid <<" alma-sv "<<gs.gpsalma.sv<<" t0a = "<<gs.gpsalma.getT0e() <<" health " <<gs.gpsalma.health;
|
||||
Point sat;
|
||||
getCoordinates(0, gs.tow, gs.gpsalma, &sat);
|
||||
getCoordinates(gs.tow, gs.gpsalma, &sat);
|
||||
TLERepo::Match second;
|
||||
auto match = tles.getBestMatch(nmm.localutcseconds(), sat.x, sat.y, sat.z, &second);
|
||||
cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
|
||||
|
|
@ -256,7 +260,7 @@ try
|
|||
if(gs.gpsalma.sv <= 24) {
|
||||
cout << " alma-sv "<<gs.gpsalma.sv<<" t0a = "<<gs.gpsalma.getT0e() <<" health " <<gs.gpsalma.health;
|
||||
Point sat;
|
||||
getCoordinates(0, gs.tow, gs.gpsalma, &sat);
|
||||
getCoordinates(gs.tow, gs.gpsalma, &sat);
|
||||
TLERepo::Match second;
|
||||
auto match = tles.getBestMatch(nmm.localutcseconds(), sat.x, sat.y, sat.z, &second);
|
||||
cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
|
||||
|
|
@ -291,11 +295,11 @@ try
|
|||
msgs[sv]=bm;
|
||||
cout<<" wn "<<bm.wn<<" t0c "<<(int)bm.t0c<<" aodc "<< (int)bm.aodc <<" aode "<< (int)bm.aode <<" sath1 "<< (int)bm.sath1 << " urai "<<(int)bm.urai << " af0 "<<bm.a0 <<" af1 " <<bm.a1 <<" af2 "<<bm.a2;
|
||||
auto offset = bm.getAtomicOffset();
|
||||
cout<< ", "<<offset.first<<"ns " << (offset.second * 3600) <<" ns/hour "<< ephAge(bm.sow, bm.t0c*8)<<endl;
|
||||
cout<< ", "<<offset.first<<"ns " << (offset.second * 3600) <<" ns/hour "<< ephAge(bm.sow, bm.t0c*8);
|
||||
}
|
||||
else if(fraid == 3 && bm.sow) {
|
||||
Point sat;
|
||||
getCoordinates(0, bm.sow, bm, &sat);
|
||||
getCoordinates(bm.sow, bm, &sat);
|
||||
TLERepo::Match second;
|
||||
auto match = tles.getBestMatch(nmm.localutcseconds(), sat.x, sat.y, sat.z, &second);
|
||||
cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
|
||||
|
|
@ -312,7 +316,7 @@ try
|
|||
if(processBeidouAlmanac(bm, bae)) {
|
||||
cout<<" alma-sv "<<bae.sv;
|
||||
Point sat;
|
||||
getCoordinates(0, bm.sow, bae.alma, &sat);
|
||||
getCoordinates(bm.sow, bae.alma, &sat);
|
||||
TLERepo::Match second;
|
||||
auto match = tles.getBestMatch(nmm.localutcseconds(), sat.x, sat.y, sat.z, &second);
|
||||
cout<<" best-tle-match "<<match.name <<" dist "<<match.distance /1000<<" km";
|
||||
|
|
@ -394,11 +398,13 @@ try
|
|||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
auto lonlat = getLongLat(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
cout<<"ECEF "<<nmm.op().x()<<", "<<nmm.op().y()<<", "<<nmm.op().z()<< " lon "<< 180*lonlat.first/M_PI << " lat "<<
|
||||
180*lonlat.second/M_PI<<endl;
|
||||
cout<<std::fixed<<"ECEF "<<nmm.op().x()<<", "<<nmm.op().y()<<", "<<nmm.op().z()<< " lon "<< 180*lonlat.first/M_PI << " lat "<<
|
||||
180*lonlat.second/M_PI<< " acc "<<nmm.op().acc()<<" m "<<endl;
|
||||
g_ourpos = Point(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<endl;
|
||||
cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<","<<(nmm.rfd().has_sigid() ? nmm.rfd().sigid() : 0) <<endl;
|
||||
|
||||
}
|
||||
else {
|
||||
cout<<"Unknown type "<< (int)nmm.type()<<endl;
|
||||
|
|
|
|||
12
navmon.proto
12
navmon.proto
|
|
@ -25,6 +25,7 @@ message NavMonMessage {
|
|||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
optional uint32 sigid = 6;
|
||||
}
|
||||
|
||||
message GPSInav {
|
||||
|
|
@ -34,6 +35,7 @@ message NavMonMessage {
|
|||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
optional uint32 sigid = 6;
|
||||
}
|
||||
|
||||
message BeidouInavD1 {
|
||||
|
|
@ -43,6 +45,7 @@ message NavMonMessage {
|
|||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
optional uint32 sigid = 6;
|
||||
}
|
||||
message BeidouInavD2 {
|
||||
required uint32 gnssWN =1;
|
||||
|
|
@ -51,6 +54,7 @@ message NavMonMessage {
|
|||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
optional uint32 sigid = 6;
|
||||
}
|
||||
|
||||
message GlonassInav {
|
||||
|
|
@ -58,12 +62,14 @@ message NavMonMessage {
|
|||
required uint32 gnssSV = 2;
|
||||
required uint32 freq = 3;
|
||||
required bytes contents = 4;
|
||||
optional uint32 sigid = 5;
|
||||
}
|
||||
|
||||
|
||||
message ReceptionData {
|
||||
required uint32 gnssID =1;
|
||||
required uint32 gnssSV =2;
|
||||
optional uint32 sigid = 7;
|
||||
required uint32 db =3;
|
||||
required uint32 el =4;
|
||||
required uint32 azi =5;
|
||||
|
|
@ -81,15 +87,15 @@ message NavMonMessage {
|
|||
required double locktimeMS = 8;
|
||||
required double doStd = 9;
|
||||
required double cpStd = 10;
|
||||
required double prStd = 11
|
||||
;
|
||||
required double prStd = 11;
|
||||
optional uint32 sigid = 12;
|
||||
}
|
||||
|
||||
message ObserverPosition {
|
||||
required double x = 1;
|
||||
required double y = 2;
|
||||
required double z = 3;
|
||||
required double accCm = 4;
|
||||
required double acc = 4;
|
||||
}
|
||||
|
||||
optional GalileoInav gi=5;
|
||||
|
|
|
|||
367
navparse.cc
367
navparse.cc
|
|
@ -29,7 +29,12 @@
|
|||
#include <Tle.h>
|
||||
using namespace std;
|
||||
|
||||
std::map<int, Point> g_srcpos;
|
||||
struct ObserverPosition
|
||||
{
|
||||
Point pos;
|
||||
time_t lastSeen{0};
|
||||
};
|
||||
std::map<int, ObserverPosition> g_srcpos;
|
||||
|
||||
template<typename T>
|
||||
class GetterSetter
|
||||
|
|
@ -399,16 +404,27 @@ void SVStat::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|||
}
|
||||
}
|
||||
|
||||
typedef std::map<pair<int,int>, SVStat> svstats_t;
|
||||
struct SatID
|
||||
{
|
||||
uint32_t gnss{255}; // these could all be 'int16_t' but leads to howling numbers of warnings with protobuf
|
||||
uint32_t sv{0};
|
||||
uint32_t sigid{0};
|
||||
bool operator<(const SatID& rhs) const
|
||||
{
|
||||
return tie(gnss, sv, sigid) < tie(rhs.gnss, rhs.sv, rhs.sigid);
|
||||
}
|
||||
};
|
||||
|
||||
typedef std::map<SatID, SVStat> svstats_t;
|
||||
svstats_t g_svstats;
|
||||
|
||||
GetterSetter<std::map<pair<int,int>, SVStat>> g_statskeeper;
|
||||
GetterSetter<svstats_t> g_statskeeper;
|
||||
|
||||
int latestWN(int gnssid, const svstats_t& stats)
|
||||
{
|
||||
map<int, pair<int,int>> ages;
|
||||
map<int, SatID> ages;
|
||||
for(const auto& s: stats)
|
||||
if(s.first.first == gnssid)
|
||||
if(s.first.gnss == (unsigned int)gnssid)
|
||||
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
|
||||
if(ages.empty())
|
||||
throw runtime_error("Asked for latest WN for "+to_string(gnssid)+": we don't know it yet");
|
||||
|
|
@ -418,9 +434,9 @@ int latestWN(int gnssid, const svstats_t& stats)
|
|||
|
||||
int latestTow(int gnssid, const svstats_t& stats)
|
||||
{
|
||||
map<int, pair<int,int>> ages;
|
||||
map<int, SatID> ages;
|
||||
for(const auto& s: stats)
|
||||
if(s.first.first == gnssid)
|
||||
if(s.first.gnss == (unsigned int) gnssid)
|
||||
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
|
||||
if(ages.empty())
|
||||
throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet");
|
||||
|
|
@ -428,7 +444,7 @@ int latestTow(int gnssid, const svstats_t& stats)
|
|||
}
|
||||
|
||||
|
||||
uint64_t nanoTime(int gnssid, int wn, int tow)
|
||||
int64_t nanoTime(int gnssid, int wn, double tow)
|
||||
{
|
||||
int offset;
|
||||
if(gnssid == 0) // GPS
|
||||
|
|
@ -460,33 +476,89 @@ struct InfluxPusher
|
|||
}
|
||||
|
||||
template<typename T>
|
||||
void addValue( const pair<pair<int,int>,SVStat>& ent, string_view name, const T& value)
|
||||
void addValue( const pair<SatID,SVStat>& ent, string_view name, const T& value)
|
||||
{
|
||||
if(d_mute)
|
||||
return;
|
||||
d_buffer+= string(name)+",gnssid="+to_string(ent.first.first)+ +",sv=" +to_string(ent.first.second)+" value="+to_string(value)+
|
||||
" "+to_string(nanoTime(ent.first.first, ent.second.wn, ent.second.tow))+"\n";
|
||||
if(nanoTime (ent.first.gnss, ent.second.wn, ent.second.tow)/1000000000 > 2000000000) {
|
||||
cerr<<"Unable to store item "<<name<<" for sv "<<ent.first.gnss<<","<<ent.first.sv<<": time out of range "<<ent.second.wn<<" "<<ent.second.tow<<endl;
|
||||
return;
|
||||
}
|
||||
d_buffer+= string(name)+",gnssid="+to_string(ent.first.gnss)+ +",sv=" +to_string(ent.first.sv)+",sigid="+to_string(ent.first.sigid);
|
||||
d_buffer+=" value="+to_string(value)+
|
||||
" "+to_string(nanoTime(ent.first.gnss, ent.second.wn, ent.second.tow))+"\n";
|
||||
checkSend();
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void addValue(pair<int,int> id, string_view name, const T& value, std::optional<int> src = std::optional<int>())
|
||||
void addValueObserver(int src, string_view name, const T& value, time_t t)
|
||||
{
|
||||
if(d_mute)
|
||||
return;
|
||||
d_buffer+= string(name)+",src="+to_string(src)+ " value="+to_string(value)+
|
||||
" "+to_string(t*1000000000)+"\n";
|
||||
checkSend();
|
||||
}
|
||||
|
||||
|
||||
template<typename T>
|
||||
void addValue(const SatID& id, string_view name, const T& value, std::optional<int> src = std::optional<int>())
|
||||
{
|
||||
if(d_mute)
|
||||
return;
|
||||
|
||||
if(nanoTime (id.gnss, g_svstats[id].wn, g_svstats[id].tow)/1000000000 > 2000000000) {
|
||||
cerr<<"Unable to store item "<<name<<" for sv "<<id.gnss<<","<<id.sv<<": time out of range "<<g_svstats[id].wn<<" "<< g_svstats[id].tow<<endl;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0)
|
||||
return;
|
||||
// cout << g_svstats[id].wn <<", "<<g_svstats[id].tow<<" -> " <<nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow)<<endl;
|
||||
d_buffer+= string(name) +",gnssid="+to_string(id.first)+",sv=" +to_string(id.second);
|
||||
d_buffer+= string(name) +",gnssid="+to_string(id.gnss)+",sv=" +to_string(id.sv)+",sigid="+to_string(id.sigid);
|
||||
if(src)
|
||||
d_buffer += ",src="+to_string(*src);
|
||||
|
||||
d_buffer+= " value="+to_string(value)+" "+
|
||||
to_string(nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
||||
to_string(nanoTime(id.gnss, g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
||||
|
||||
checkSend();
|
||||
}
|
||||
|
||||
void addValue(const SatID& id, uint64_t nanotime, string_view name, const initializer_list<pair<const char*, double>>& values, std::optional<int> src = std::optional<int>())
|
||||
{
|
||||
if(d_mute)
|
||||
return;
|
||||
|
||||
if(nanotime/1000000000 > 2000000000) {
|
||||
cerr<<"Unable to store item "<<name<<" for sv "<<id.gnss<<","<<id.sv<<": time out of range "<<nanotime;
|
||||
return;
|
||||
}
|
||||
for(const auto& p : values) {
|
||||
if(isnan(p.second))
|
||||
return;
|
||||
}
|
||||
|
||||
d_buffer+= string(name) +",gnssid="+to_string(id.gnss)+",sv=" +to_string(id.sv)+",sigid="+to_string(id.sigid);
|
||||
if(src)
|
||||
d_buffer += ",src="+to_string(*src);
|
||||
|
||||
d_buffer+= " ";
|
||||
bool lefirst=true;
|
||||
for(const auto& v : values) {
|
||||
if(!lefirst) {
|
||||
d_buffer +=",";
|
||||
}
|
||||
lefirst=false;
|
||||
d_buffer += string(v.first) + "="+to_string(v.second);
|
||||
}
|
||||
d_buffer += " " + to_string(nanotime)+"\n";
|
||||
|
||||
checkSend();
|
||||
}
|
||||
|
||||
|
||||
void checkSend()
|
||||
{
|
||||
if(d_buffer.size() > 1000000 || (time(0) - d_lastsent) > 10) {
|
||||
|
|
@ -546,11 +618,11 @@ std::optional<double> getHzCorrection(time_t now, const svstats_t svstats)
|
|||
if(now - s.second.deltaHz.first < 60) {
|
||||
alltot+=s.second.deltaHz.second;
|
||||
allcount++;
|
||||
if(s.first.first == 0) {
|
||||
if(s.first.gnss == 0) {
|
||||
gpstot+=s.second.deltaHz.second;
|
||||
gpscount++;
|
||||
}
|
||||
else if(s.first.first == 2) {
|
||||
else if(s.first.gnss == 2 && s.first.sigid == 1) { // focus on E1
|
||||
galtot+=s.second.deltaHz.second;
|
||||
galcount++;
|
||||
}
|
||||
|
|
@ -583,20 +655,6 @@ char getGNSSChar(int id)
|
|||
return '0'+id;
|
||||
}
|
||||
|
||||
double getElevationDeg(const Point& p, int sourceid)
|
||||
{
|
||||
Point our = g_srcpos[sourceid];
|
||||
Point core{0,0,0};
|
||||
|
||||
Vector core2us(core, our);
|
||||
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
||||
// https://ds9a.nl/articles/
|
||||
|
||||
double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
|
||||
double deg = 180.0* (elev/M_PI);
|
||||
return 90.0 - deg;
|
||||
}
|
||||
|
||||
std::string humanBhs(int bhs)
|
||||
{
|
||||
|
|
@ -631,7 +689,7 @@ try
|
|||
catch(...)
|
||||
{}
|
||||
|
||||
map<int, int> utcstats, gpsgststats, gpsutcstats;
|
||||
map<int, SatID> utcstats, gpsgststats, gpsutcstats;
|
||||
|
||||
for(const auto& s: svstats) {
|
||||
if(!s.second.wn) // this will suck in 20 years
|
||||
|
|
@ -640,41 +698,40 @@ try
|
|||
//Galileo-UTC offset: 3.22 ns, Galileo-GPS offset: 7.67 ns, 18 leap seconds
|
||||
|
||||
|
||||
if(s.first.first == 0) { // GPS
|
||||
int sv = s.first.second;
|
||||
int dw = (uint8_t)svstats[{0,sv}].wn - svstats[{0,sv}].wn0t;
|
||||
int age = dw * 7 * 86400 + svstats[{0,sv}].tow - svstats[{0,sv}].t0t; // t0t is PRESCALED
|
||||
if(s.first.gnss == 0) { // GPS
|
||||
int dw = (uint8_t)s.second.wn - s.second.wn0t;
|
||||
int age = dw * 7 * 86400 + s.second.tow - s.second.t0t; // t0t is PRESCALED // XXX changed this,
|
||||
|
||||
gpsutcstats[age]=s.first.second;
|
||||
gpsutcstats[age]=s.first;
|
||||
continue;
|
||||
}
|
||||
|
||||
int dw = (uint8_t)s.second.wn - s.second.wn0t;
|
||||
int age = dw * 7 * 86400 + s.second.tow - s.second.t0t; // t0t is pre-scaled
|
||||
utcstats[age]=s.first.second;
|
||||
utcstats[age]=s.first;
|
||||
|
||||
uint8_t wn0g = s.second.wn0t;
|
||||
int dwg = (((uint8_t)s.second.wn)&(1+2+4+8+16+32)) - wn0g;
|
||||
age = dwg*7*86400 + s.second.tow - s.second.t0g * 3600;
|
||||
gpsgststats[age]=s.first.second;
|
||||
gpsgststats[age]=s.first;
|
||||
|
||||
}
|
||||
if(utcstats.empty()) {
|
||||
ret["utc-offset-ns"]=nullptr;
|
||||
}
|
||||
else {
|
||||
int sv = utcstats.begin()->second; // freshest SV
|
||||
long shift = svstats[{2,sv}].a0 * (1LL<<20) + svstats[{2,sv}].a1 * utcstats.begin()->first; // in 2^-50 seconds units
|
||||
auto satid = utcstats.begin()->second; // freshest SV
|
||||
long shift = svstats[{2,satid.sv,satid.sigid}].a0 * (1LL<<20) + svstats[{2,satid.sv,satid.sigid}].a1 * utcstats.begin()->first; // in 2^-50 seconds units
|
||||
ret["utc-offset-ns"] = 1.073741824*ldexp(1.0*shift, -20);
|
||||
ret["leap-second-planned"] = (svstats[{2,sv}].dtLSF != svstats[{2,sv}].dtLS);
|
||||
ret["leap-second-planned"] = (svstats[satid].dtLSF != svstats[satid].dtLS);
|
||||
}
|
||||
|
||||
if(gpsgststats.empty()) {
|
||||
ret["gps-offset-ns"]=nullptr;
|
||||
}
|
||||
else {
|
||||
int sv = gpsgststats.begin()->second; // freshest SV
|
||||
long shift = svstats[{2,sv}].a0g * (1L<<16) + svstats[{2,sv}].a1g * gpsgststats.begin()->first; // in 2^-51 seconds units
|
||||
auto satid = gpsgststats.begin()->second; // freshest SV
|
||||
long shift = svstats[{2,satid.sv, satid.sigid}].a0g * (1L<<16) + svstats[{2,satid.sv, satid.sigid}].a1g * gpsgststats.begin()->first; // in 2^-51 seconds units
|
||||
|
||||
ret["gps-offset-ns"] = 1.073741824*ldexp(shift, -21);
|
||||
}
|
||||
|
|
@ -683,8 +740,8 @@ try
|
|||
ret["gps-utc-offset-ns"]=nullptr;
|
||||
}
|
||||
else {
|
||||
int sv = gpsutcstats.begin()->second; // freshest SV
|
||||
long shift = svstats[{0,sv}].a0 * (1LL<<20) + svstats[{0,sv}].a1 * gpsutcstats.begin()->first; // In 2^-50 seconds units
|
||||
auto satid = gpsutcstats.begin()->second; // freshest SV
|
||||
long shift = svstats[{0,satid.sv,satid.sigid}].a0 * (1LL<<20) + svstats[{0,satid.sv,satid.sigid}].a1 * gpsutcstats.begin()->first; // In 2^-50 seconds units
|
||||
|
||||
ret["gps-utc-offset-ns"] = 1.073741824*ldexp(shift, -20);
|
||||
}
|
||||
|
|
@ -705,7 +762,7 @@ try
|
|||
if(ae.second.alma.getT0e() > 7*86400)
|
||||
continue;
|
||||
Point sat;
|
||||
getCoordinates(0, latestTow(3, svstats), ae.second.alma, &sat);
|
||||
getCoordinates(latestTow(3, svstats), ae.second.alma, &sat);
|
||||
item["eph-ecefX"]= sat.x/1000;
|
||||
item["eph-ecefY"]= sat.y/1000;
|
||||
item["eph-ecefZ"]= sat.z/1000;
|
||||
|
|
@ -716,6 +773,13 @@ try
|
|||
item["t0e"] = ae.second.alma.getT0e();
|
||||
item["t"]= ephAge(ae.second.alma.getT0e(), latestTow(3, svstats))/86400.0;
|
||||
item["inclination"] = 180 * ae.second.alma.getI0() /M_PI;
|
||||
|
||||
item["observed"]=false;
|
||||
if(auto iter = svstats.find({3, (uint32_t)ae.first, 0}); iter != svstats.end()) {
|
||||
if(time(0) - nanoTime(3, iter->second.wn, iter->second.tow)/1000000000 < 300)
|
||||
item["observed"] = true;
|
||||
}
|
||||
|
||||
if(ephAge(ae.second.alma.getT0e(), latestTow(3, svstats)) < 0) {
|
||||
auto match = g_tles.getBestMatch(nanoTime(3, latestWN(3, svstats), latestTow(3, svstats))/1000000000.0,
|
||||
sat.x, sat.y, sat.z);
|
||||
|
|
@ -756,6 +820,22 @@ try
|
|||
item["lambdana"] = ae.second.second.getLambdaNaDeg();
|
||||
item["hna"] = ae.second.second.hna;
|
||||
|
||||
item["observed"] = false;
|
||||
for(uint32_t sigid : {0,1,2}) { // XXX SIGIDS
|
||||
if(auto iter = svstats.find({6, (uint32_t)ae.first, sigid}); iter != svstats.end()) {
|
||||
if(time(0) - nanoTime(6, iter->second.wn, iter->second.tow)/1000000000 < 300) {
|
||||
item["observed"] = true;
|
||||
auto longlat = getLongLat(iter->second.glonassMessage.x, iter->second.glonassMessage.y, iter->second.glonassMessage.z);
|
||||
item["eph-longitude"] = 180*longlat.first/M_PI;
|
||||
item["eph-latitude"]= 180*longlat.second/M_PI;
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
ret[fmt::sprintf("R%02d", ae.first)] = item;
|
||||
}
|
||||
|
||||
|
|
@ -772,7 +852,7 @@ try
|
|||
item["i0"] = 180.0 * ae.second.getI0()/ M_PI;
|
||||
item["inclination"] = 180 * ae.second.getI0() /M_PI;
|
||||
Point sat;
|
||||
getCoordinates(0, latestTow(2, svstats), ae.second, &sat);
|
||||
getCoordinates(latestTow(2, svstats), ae.second, &sat);
|
||||
item["eph-ecefX"]= sat.x/1000;
|
||||
item["eph-ecefY"]= sat.y/1000;
|
||||
item["eph-ecefZ"]= sat.z/1000;
|
||||
|
|
@ -781,6 +861,16 @@ try
|
|||
item["eph-longitude"] = 180*longlat.first/M_PI;
|
||||
item["eph-latitude"]= 180*longlat.second/M_PI;
|
||||
|
||||
|
||||
item["observed"] = false;
|
||||
for(uint32_t sigid : {0,1,5}) {
|
||||
if(auto iter = svstats.find({2, (uint32_t)ae.first, sigid}); iter != svstats.end()) {
|
||||
if(time(0) - nanoTime(2, iter->second.wn, iter->second.tow)/1000000000 < 300)
|
||||
item["observed"] = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
auto match = g_tles.getBestMatch(nanoTime(2, latestWN(2, svstats), latestTow(2, svstats))/1000000000.0,
|
||||
sat.x, sat.y, sat.z);
|
||||
|
||||
|
|
@ -817,7 +907,7 @@ try
|
|||
item["i0"] = 180.0 * ae.second.getI0()/ M_PI;
|
||||
item["inclination"] = 180 * ae.second.getI0() /M_PI;
|
||||
Point sat;
|
||||
getCoordinates(0, latestTow(0, svstats), ae.second, &sat);
|
||||
getCoordinates(latestTow(0, svstats), ae.second, &sat);
|
||||
item["eph-ecefX"]= sat.x/1000;
|
||||
item["eph-ecefY"]= sat.y/1000;
|
||||
item["eph-ecefZ"]= sat.z/1000;
|
||||
|
|
@ -826,6 +916,15 @@ try
|
|||
item["eph-longitude"] = 180*longlat.first/M_PI;
|
||||
item["eph-latitude"]= 180*longlat.second/M_PI;
|
||||
|
||||
item["observed"] = false;
|
||||
for(uint32_t sigid : {0,1,4}) {
|
||||
if(auto iter = svstats.find({0, (uint32_t)ae.first, sigid}); iter != svstats.end()) {
|
||||
if(time(0) - nanoTime(0, iter->second.wn, iter->second.tow)/1000000000 < 300)
|
||||
item["observed"] = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
auto match = g_tles.getBestMatch(nanoTime(0, latestWN(0, svstats), latestTow(0, svstats))/1000000000.0,
|
||||
sat.x, sat.y, sat.z);
|
||||
|
||||
|
|
@ -858,13 +957,14 @@ try
|
|||
for(const auto& src : g_srcpos) {
|
||||
nlohmann::json obj;
|
||||
obj["id"] = src.first;
|
||||
auto longlat = getLongLat(src.second.x, src.second.y, src.second.z);
|
||||
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;
|
||||
obj["last-seen"] = src.second.lastSeen;
|
||||
ret.push_back(obj);
|
||||
}
|
||||
return ret;
|
||||
|
|
@ -881,11 +981,12 @@ try
|
|||
if(!s.second.tow) // I know, I know, will suck briefly
|
||||
continue;
|
||||
|
||||
item["gnssid"] = s.first.first;
|
||||
item["svid"] = s.first.second;
|
||||
item["gnssid"] = s.first.gnss;
|
||||
item["svid"] = s.first.sv;
|
||||
item["sigid"] = s.first.sigid;
|
||||
// perhaps check oldBeidouMessage for sow >=0 as 'completeIOD'?
|
||||
|
||||
if(s.first.first == 3) { // beidou
|
||||
if(s.first.gnss == 3) { // beidou
|
||||
item["sisa"]=humanUra(s.second.ura);
|
||||
if(s.second.t0eMSB >= 0 && s.second.t0eLSB >=0)
|
||||
item["eph-age-m"] = ephAge(s.second.tow, 8.0*((s.second.t0eMSB<<15) + s.second.t0eLSB))/60.0;
|
||||
|
|
@ -902,15 +1003,15 @@ try
|
|||
item["best-tle-int-desig"] = s.second.tleMatch.internat;
|
||||
}
|
||||
Point p;
|
||||
getCoordinates(0, s.second.tow, s.second.oldBeidouMessage, &p);
|
||||
getCoordinates(s.second.tow, s.second.oldBeidouMessage, &p);
|
||||
auto beidoualma = g_beidoualmakeeper.get();
|
||||
if(auto iter = beidoualma.find(s.first.second); iter != beidoualma.end()) {
|
||||
if(auto iter = beidoualma.find(s.first.sv); iter != beidoualma.end()) {
|
||||
Point almapos;
|
||||
getCoordinates(0, s.second.tow, iter->second.alma, &almapos);
|
||||
getCoordinates(s.second.tow, iter->second.alma, &almapos);
|
||||
item["alma-dist"] = Vector(almapos, p).length()/1000.0;
|
||||
}
|
||||
}
|
||||
else if(s.first.first == 6) { // glonass
|
||||
else if(s.first.gnss == 6) { // glonass
|
||||
if(s.second.glonassMessage.FT < 16)
|
||||
item["sisa"] = humanFt(s.second.glonassMessage.FT);
|
||||
item["aode"] = s.second.aode;
|
||||
|
|
@ -933,7 +1034,7 @@ try
|
|||
}
|
||||
if(s.second.completeIOD()) {
|
||||
item["iod"]=s.second.getIOD();
|
||||
if(s.first.first == 0 || s.first.first == 3) {
|
||||
if(s.first.gnss == 0 || s.first.gnss == 3) {
|
||||
item["sisa"]=humanUra(s.second.ura);
|
||||
// cout<<"Asked to convert "<<s.second.ura<<" for sv "<<s.first.first<<","<<s.first.second<<endl;
|
||||
}
|
||||
|
|
@ -950,9 +1051,9 @@ try
|
|||
Point core;
|
||||
|
||||
// this should actually use local time!
|
||||
getCoordinates(0, s.second.tow, s.second.liveIOD(), &p);
|
||||
getCoordinates(s.second.tow, s.second.liveIOD(), &p);
|
||||
auto match = g_tles.getBestMatch(
|
||||
s.first.first ?
|
||||
s.first.gnss ?
|
||||
utcFromGST((int)s.second.wn, (int)s.second.tow) : utcFromGPS(s.second.wn, s.second.tow),
|
||||
p.x, p.y, p.z);
|
||||
|
||||
|
|
@ -971,19 +1072,19 @@ try
|
|||
if(hzCorrection)
|
||||
item["delta_hz_corr"] = s.second.deltaHz.second - *hzCorrection;
|
||||
}
|
||||
if(s.first.first == 0) {
|
||||
if(s.first.gnss == 0) {
|
||||
auto gpsalma = g_gpsalmakeeper.get();
|
||||
if(auto iter = gpsalma.find(s.first.second); iter != gpsalma.end()) {
|
||||
if(auto iter = gpsalma.find(s.first.sv); iter != gpsalma.end()) {
|
||||
Point almapos;
|
||||
getCoordinates(0, s.second.tow, iter->second, &almapos);
|
||||
getCoordinates(s.second.tow, iter->second, &almapos);
|
||||
item["alma-dist"] = Vector(almapos, p).length()/1000.0;
|
||||
}
|
||||
}
|
||||
if(s.first.first == 2) {
|
||||
if(s.first.gnss == 2) {
|
||||
auto galileoalma = g_galileoalmakeeper.get();
|
||||
if(auto iter = galileoalma.find(s.first.second); iter != galileoalma.end()) {
|
||||
if(auto iter = galileoalma.find(s.first.sv); iter != galileoalma.end()) {
|
||||
Point almapos;
|
||||
getCoordinates(0, s.second.tow, iter->second, &almapos);
|
||||
getCoordinates(s.second.tow, iter->second, &almapos);
|
||||
item["alma-dist"] = Vector(almapos, p).length()/1000.0;
|
||||
}
|
||||
}
|
||||
|
|
@ -993,13 +1094,13 @@ try
|
|||
|
||||
item["a0"]=s.second.a0;
|
||||
item["a1"]=s.second.a1;
|
||||
if(s.first.first == 0) { // GPS
|
||||
if(s.first.gnss == 0) { // GPS
|
||||
auto deltaUTC = getGPSUTCOffset(s.second.tow, s.second.wn, s.second);
|
||||
item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400);
|
||||
item["t0t"] = s.second.t0t;
|
||||
item["wn0t"] = s.second.wn0t;
|
||||
}
|
||||
if(s.first.first == 2) {
|
||||
if(s.first.gnss == 2) {
|
||||
auto deltaUTC = s.second.galmsg.getUTCOffset(s.second.tow, s.second.wn);
|
||||
item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400);
|
||||
auto deltaGPS = s.second.galmsg.getGPSOffset(s.second.tow, s.second.wn);
|
||||
|
|
@ -1007,7 +1108,7 @@ try
|
|||
item["t0t"] = s.second.galmsg.t0t;
|
||||
item["wn0t"] = s.second.galmsg.wn0t;
|
||||
}
|
||||
if(s.first.first == 3) {
|
||||
if(s.first.gnss == 3) {
|
||||
auto deltaUTC = s.second.oldBeidouMessage.getUTCOffset(s.second.oldBeidouMessage.sow);
|
||||
item["delta-utc"] = fmt::sprintf("%.1f %+.1f/d", deltaUTC.first, deltaUTC.second * 86400);
|
||||
|
||||
|
|
@ -1024,7 +1125,7 @@ try
|
|||
|
||||
item["dtLS"]=s.second.dtLS;
|
||||
|
||||
if(s.first.first == 3) { // beidou
|
||||
if(s.first.gnss == 3) { // beidou
|
||||
item["a0g"]=s.second.a0g;
|
||||
item["a1g"]=s.second.a1g;
|
||||
if(s.second.aode >= 0)
|
||||
|
|
@ -1035,7 +1136,7 @@ try
|
|||
}
|
||||
|
||||
|
||||
if(s.first.first == 2) { // galileo
|
||||
if(s.first.gnss == 2) { // galileo
|
||||
item["a0g"]=s.second.a0g;
|
||||
item["a1g"]=s.second.a1g;
|
||||
item["t0g"]=s.second.t0g;
|
||||
|
|
@ -1059,23 +1160,25 @@ try
|
|||
item["healthissue"] = 2;
|
||||
|
||||
}
|
||||
else if(s.first.first == 0 || s.first.first == 3 || s.first.first == 6) {// gps or beidou or GLONASS
|
||||
else if(s.first.gnss == 0 || s.first.gnss == 3 || s.first.gnss == 6) {// gps or beidou or GLONASS
|
||||
item["health"]= s.second.gpshealth ? ("NOT OK: "+to_string(s.second.gpshealth)) : string("OK");
|
||||
item["healthissue"]= 2* !!s.second.gpshealth;
|
||||
}
|
||||
|
||||
nlohmann::json perrecv = nlohmann::json::object();
|
||||
for(const auto& pr : s.second.perrecv) {
|
||||
if(pr.second.el > 0 && pr.second.el <= 90) {
|
||||
if(pr.second.db > 0) {
|
||||
nlohmann::json det = nlohmann::json::object();
|
||||
det["elev"] = pr.second.el;
|
||||
Point sat;
|
||||
|
||||
if((s.first.first == 0 || s.first.first == 2) && s.second.completeIOD())
|
||||
getCoordinates(0, latestTow(s.first.first, svstats), s.second.liveIOD(), & sat);
|
||||
if((s.first.gnss == 0 || s.first.gnss == 2) && s.second.completeIOD())
|
||||
getCoordinates(latestTow(s.first.gnss, svstats), s.second.liveIOD(), & sat);
|
||||
|
||||
if(sat.x) {
|
||||
det["elev"] = getElevationDeg(sat, pr.first);
|
||||
Point our = g_srcpos[pr.first].pos;
|
||||
det["elev"] = getElevationDeg(sat, our);
|
||||
det["azi"] = getAzimuthDeg(sat, our);
|
||||
}
|
||||
else
|
||||
det["elev"] = pr.second.el;
|
||||
|
|
@ -1087,7 +1190,8 @@ try
|
|||
}
|
||||
item["perrecv"]=perrecv;
|
||||
|
||||
item["last-seen-s"] = time(0) - nanoTime(s.first.first, s.second.wn, s.second.tow)/1000000000;
|
||||
item["last-seen-s"] = time(0) - nanoTime(s.first.gnss, s.second.wn, s.second.tow)/1000000000;
|
||||
|
||||
if(s.second.latestDisco >=0) {
|
||||
item["latest-disco"]= s.second.latestDisco;
|
||||
}
|
||||
|
|
@ -1098,7 +1202,7 @@ try
|
|||
|
||||
item["wn"] = s.second.wn;
|
||||
item["tow"] = s.second.tow;
|
||||
ret[fmt::sprintf("%c%02d", getGNSSChar(s.first.first), s.first.second)] = item;
|
||||
ret[fmt::sprintf("%c%02d@%d", getGNSSChar(s.first.gnss), s.first.sv, s.first.sigid)] = item;
|
||||
}
|
||||
return ret;
|
||||
});
|
||||
|
|
@ -1144,22 +1248,43 @@ try
|
|||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
int gnssid = nmm.rd().gnssid();
|
||||
int sv = nmm.rd().gnsssv();
|
||||
pair<int,int> id{gnssid, sv};
|
||||
int sigid = nmm.rd().sigid();
|
||||
if(gnssid==2 && sigid == 0)
|
||||
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();
|
||||
|
||||
// THIS HAS TO SPLIT OUT PER SOURCE
|
||||
idb.addValue(id, "db", nmm.rd().db(), nmm.sourceid());
|
||||
if(nmm.rd().el() <= 90 && nmm.rd().el() > 0)
|
||||
idb.addValue(id, "elev", nmm.rd().el(), nmm.sourceid());
|
||||
idb.addValue(id, "azi", nmm.rd().azi(), nmm.sourceid());
|
||||
idb.addValue(id, "prres", nmm.rd().prres(), nmm.sourceid());
|
||||
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
|
||||
if(id.gnss == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
|
||||
getCoordinates(g_svstats[id].tow, g_svstats[id].oldBeidouMessage, &sat);
|
||||
}
|
||||
else if(g_svstats[id].completeIOD()) {
|
||||
getCoordinates(g_svstats[id].tow, g_svstats[id].liveIOD(), &sat);
|
||||
}
|
||||
if(sat.x != 0) {
|
||||
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());
|
||||
}
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
int sv = nmm.gi().gnsssv();
|
||||
pair<int, int> id={2,sv};
|
||||
int sigid;
|
||||
if(nmm.gi().has_sigid())
|
||||
sigid = nmm.gi().sigid();
|
||||
else
|
||||
sigid = 1; // default to E1B
|
||||
SatID id={2,(uint32_t)sv,(uint32_t)sigid};
|
||||
g_svstats[id].wn = nmm.gi().gnsswn();
|
||||
auto& svstat = g_svstats[id];
|
||||
auto oldgm = svstat.galmsg;
|
||||
|
|
@ -1175,7 +1300,7 @@ try
|
|||
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.first, svstat.wn, svstat.tow)<<" Galileo "<<id.second <<" 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)<<" 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1294,14 +1419,14 @@ try
|
|||
int ephage = ephAge(ent.second.tow, ent.second.prevIOD.second.t0e);
|
||||
if(ent.second.liveIOD().sisa != ent.second.prevIOD.second.sisa) {
|
||||
|
||||
cout<<humanTime(id.first, ent.second.wn, ent.second.tow)<<" gnssid "<<ent.first.first<<" sv "<<ent.first.second<<" changed sisa from "<<(unsigned int) ent.second.prevIOD.second.sisa<<" ("<<
|
||||
cout<<humanTime(id.gnss, ent.second.wn, ent.second.tow)<<" gnssid "<<ent.first.gnss<<" sv "<<ent.first.sv<<"@"<<ent.first.sigid<<" changed sisa from "<<(unsigned int) ent.second.prevIOD.second.sisa<<" ("<<
|
||||
humanSisa(ent.second.prevIOD.second.sisa)<<") to " << (unsigned int)ent.second.liveIOD().sisa << " ("<<
|
||||
humanSisa(ent.second.liveIOD().sisa)<<"), lastseen = "<< (ephage/3600.0) <<"h"<<endl;
|
||||
}
|
||||
|
||||
Point p, oldp;
|
||||
getCoordinates(ent.second.wn, ent.second.tow, ent.second.liveIOD(), &p);
|
||||
getCoordinates(ent.second.wn, ent.second.tow, ent.second.prevIOD.second, &oldp);
|
||||
getCoordinates(ent.second.tow, ent.second.liveIOD(), &p);
|
||||
getCoordinates(ent.second.tow, ent.second.prevIOD.second, &oldp);
|
||||
|
||||
if(ent.second.prevIOD.second.t0e < ent.second.liveIOD().t0e) {
|
||||
double hours = ((ent.second.liveIOD().t0e - ent.second.prevIOD.second.t0e)/3600.0);
|
||||
|
|
@ -1320,15 +1445,15 @@ try
|
|||
|
||||
|
||||
if(0 && hours < 2) {
|
||||
ofstream orbitcsv("orbit."+to_string(id.first)+"."+to_string(id.second)+"."+to_string(ent.second.prevIOD.first)+"-"+to_string(ent.second.getIOD())+".csv");
|
||||
ofstream orbitcsv("orbit."+to_string(id.gnss)+"."+to_string(id.sv)+"."+to_string(ent.second.prevIOD.first)+"-"+to_string(ent.second.getIOD())+".csv");
|
||||
|
||||
orbitcsv << "timestamp x y z oldx oldy oldz\n";
|
||||
orbitcsv << fixed;
|
||||
for(int offset = -7200; offset < 7200; offset += 30) {
|
||||
int t = ent.second.liveIOD().t0e + offset;
|
||||
Point p, oldp;
|
||||
getCoordinates(ent.second.wn, t, ent.second.liveIOD(), &p);
|
||||
getCoordinates(ent.second.wn, t, ent.second.prevIOD.second, &oldp);
|
||||
getCoordinates(t, ent.second.liveIOD(), &p);
|
||||
getCoordinates(t, ent.second.prevIOD.second, &oldp);
|
||||
time_t posix = utcFromGST(ent.second.wn, t);
|
||||
orbitcsv << posix <<" "
|
||||
<<p.x<<" " <<p.y<<" "<<p.z<<" "
|
||||
|
|
@ -1341,17 +1466,24 @@ try
|
|||
}
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
g_srcpos[nmm.sourceid()].x = nmm.op().x();
|
||||
g_srcpos[nmm.sourceid()].y = nmm.op().y();
|
||||
g_srcpos[nmm.sourceid()].z = nmm.op().z();
|
||||
g_srcpos[nmm.sourceid()].lastSeen = nmm.localutcseconds();
|
||||
g_srcpos[nmm.sourceid()].pos.x = nmm.op().x();
|
||||
g_srcpos[nmm.sourceid()].pos.y = nmm.op().y();
|
||||
g_srcpos[nmm.sourceid()].pos.z = nmm.op().z();
|
||||
idb.addValueObserver(nmm.sourceid(), "accfix", nmm.op().acc(), nmm.localutcseconds());
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
pair<int,int> id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()};
|
||||
if(id.first == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
|
||||
auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()], g_svstats[id].oldBeidouMessage, 1561.098 * 1000000);
|
||||
SatID id{nmm.rfd().gnssid(), nmm.rfd().gnsssv(), nmm.rfd().sigid()};
|
||||
idb.addValue(id, nanoTime(0, nmm.rfd().rcvwn(), nmm.rfd().rcvtow()), "rfdata",
|
||||
{{"carrierphase", nmm.rfd().carrierphase()},
|
||||
{"doppler", nmm.rfd().doppler()},
|
||||
{"locktime", nmm.rfd().locktimems()},
|
||||
{"pseudorange", nmm.rfd().pseudorange()}});
|
||||
if(id.gnss == 3 && g_svstats[id].oldBeidouMessage.sow >= 0 && g_svstats[id].oldBeidouMessage.sqrtA != 0) {
|
||||
auto res = doDoppler(nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()].pos, g_svstats[id].oldBeidouMessage, 1561.098 * 1000000);
|
||||
|
||||
if(isnan(res.preddop)) {
|
||||
cerr<<"Problem with doppler calculation for C"<<id.second<<": "<<endl;
|
||||
cerr<<"Problem with doppler calculation for C"<<id.sv<<": "<<endl;
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
|
@ -1367,7 +1499,18 @@ try
|
|||
}
|
||||
}
|
||||
else if(g_svstats[id].completeIOD()) {
|
||||
auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()], g_svstats[id].liveIOD(), 1575.42 * 1000000);
|
||||
double freqMHZ = 1575.42;
|
||||
if(id.gnss == 2 && id.sigid == 5)
|
||||
freqMHZ = 1207.140;
|
||||
|
||||
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}});
|
||||
|
||||
|
||||
|
||||
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
||||
|
||||
if(t - g_svstats[id].deltaHz.first > 10) {
|
||||
|
|
@ -1381,8 +1524,12 @@ try
|
|||
}
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::GPSInavType) {
|
||||
if(nmm.gpsi().sigid()) {
|
||||
cout<<"ignoring sigid "<<nmm.gpsi().sigid()<<" for GPS "<<nmm.gpsi().gnsssv()<<endl;
|
||||
continue;
|
||||
}
|
||||
auto cond = getCondensedGPSMessage(std::basic_string<uint8_t>((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
pair<int,int> id{nmm.gpsi().gnssid(), nmm.gpsi().gnsssv()};
|
||||
SatID id{nmm.gpsi().gnssid(), nmm.gpsi().gnsssv(), nmm.gpsi().sigid()};
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
|
|
@ -1399,11 +1546,11 @@ try
|
|||
idb.addValue(id, "ura", svstat.ura);
|
||||
|
||||
if(oldsvstat.af0 && oldsvstat.ura != svstat.ura && svstat.ura > 1) { // XX find better way to check
|
||||
cout<<humanTime(id.first, svstat.wn, svstat.tow)<<" wn "<<svstat.wn <<" GPS "<<id.second <<" 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)<<" 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.first, svstat.wn, svstat.tow)<<" wn "<<svstat.wn <<" GPS "<<id.second <<" 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)<<" 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;
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -1421,7 +1568,7 @@ try
|
|||
}
|
||||
else if(frame==2) {
|
||||
if(oldsvstat.gpshealth != svstat.gpshealth) {
|
||||
cout<<humanTime(id.first, svstat.wn, svstat.tow)<<" GPS "<<id.second <<" 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)<<" 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);
|
||||
}
|
||||
|
|
@ -1444,7 +1591,7 @@ try
|
|||
g_svstats[id].wn += 2048;
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD1) {
|
||||
pair<int,int> id{nmm.bid1().gnssid(), nmm.bid1().gnsssv()};
|
||||
SatID id{nmm.bid1().gnssid(), nmm.bid1().gnsssv(), nmm.bid1().sigid()};
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
|
|
@ -1466,7 +1613,7 @@ try
|
|||
svstat.aodc = bm.aodc;
|
||||
|
||||
if(oldbm.sath1 != bm.sath1) {
|
||||
cout<<humanTime(id.first, svstat.wn, svstat.tow)<<" BeiDou C"<<id.second<<" health changed from "<<(int)oldbm.sath1 <<" to "<< (int)bm.sath1 <<", lastseen "<<ephAge(oldbm.sow, bm.sow)/3600.0<<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" BeiDou C"<<id.sv<<"@"<<id.sigid<<" health changed from "<<(int)oldbm.sath1 <<" to "<< (int)bm.sath1 <<", lastseen "<<ephAge(oldbm.sow, bm.sow)/3600.0<<endl;
|
||||
}
|
||||
|
||||
idb.addValue(id, "atomic_offset_ns", 1000000.0*bm.getAtomicOffset().first);
|
||||
|
|
@ -1491,7 +1638,7 @@ try
|
|||
svstat.t0eLSB = bm.t0eLSB;
|
||||
Point oldpoint, newpoint;
|
||||
if(bm.sow - svstat.lastBeidouMessage2.sow == 6 && svstat.oldBeidouMessage.sow >= 0) {
|
||||
getCoordinates(svstat.wn, svstat.tow, bm, &newpoint);
|
||||
getCoordinates(svstat.tow, bm, &newpoint);
|
||||
|
||||
if(fabs(svstat.lastTLELookupX - newpoint.x) > 300000) {
|
||||
// cout<<"fraid 3 lookup, delta " << fabs(svstat.lastTLELookupX - newpoint.x) << endl;
|
||||
|
|
@ -1501,7 +1648,7 @@ try
|
|||
}
|
||||
|
||||
if(svstat.oldBeidouMessage.getT0e() != svstat.beidouMessage.getT0e()) {
|
||||
getCoordinates(svstat.wn, svstat.tow, svstat.oldBeidouMessage, &oldpoint);
|
||||
getCoordinates(svstat.tow, svstat.oldBeidouMessage, &oldpoint);
|
||||
Vector jump(oldpoint ,newpoint);
|
||||
/* cout<<fmt::sprintf("Discontinuity C%02d (%f,%f,%f) -> (%f, %f, %f), jump: %f, seconds: %f\n",
|
||||
id.second, oldpoint.x, oldpoint.y, oldpoint.z,
|
||||
|
|
@ -1559,7 +1706,7 @@ try
|
|||
*/
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::GlonassInavType) {
|
||||
pair<int,int> id{nmm.gloi().gnssid(), nmm.gloi().gnsssv()};
|
||||
SatID id{nmm.gloi().gnssid(), nmm.gloi().gnsssv(), nmm.gloi().sigid()};
|
||||
auto& svstat = g_svstats[id];
|
||||
|
||||
auto& gm = svstat.glonassMessage;
|
||||
|
|
@ -1574,7 +1721,7 @@ try
|
|||
}
|
||||
else if(strno == 2) {
|
||||
if(oldgm.Bn != gm.Bn) {
|
||||
cout<<humanTime(id.first, svstat.wn, svstat.tow)<<" GLONASS R"<<id.second<<" health changed from "<<(int)oldgm.Bn <<" to "<< (int)gm.Bn <<endl;
|
||||
cout<<humanTime(id.gnss, svstat.wn, svstat.tow)<<" GLONASS R"<<id.sv<<"@"<<id.sigid<<" health changed from "<<(int)oldgm.Bn <<" to "<< (int)gm.Bn <<endl;
|
||||
|
||||
}
|
||||
svstat.gpshealth = gm.Bn;
|
||||
|
|
|
|||
157
ubxtool.cc
157
ubxtool.cc
|
|
@ -368,7 +368,7 @@ int main(int argc, char** argv)
|
|||
int fd = initFD(serial[0].c_str(), doRTSCTS);
|
||||
|
||||
g_srcid = atoi(serial[1].c_str());
|
||||
|
||||
bool version9 = false;
|
||||
if(!g_fromFile) {
|
||||
bool doInit = true;
|
||||
if(doInit) {
|
||||
|
|
@ -406,8 +406,15 @@ int main(int argc, char** argv)
|
|||
UBXMessage um1=waitForUBX(fd, 2, 0x0a, 0x04);
|
||||
cerr<<"swVersion: "<<um1.getPayload().c_str()<<endl;
|
||||
cerr<<"hwVersion: "<<um1.getPayload().c_str()+30<<endl;
|
||||
for(unsigned int n=0; 40+30*n < um1.getPayload().size(); ++n)
|
||||
|
||||
for(unsigned int n=0; 40+30*n < um1.getPayload().size(); ++n) {
|
||||
cerr<<"Extended info: "<<um1.getPayload().c_str() + 40 +30*n<<endl;
|
||||
if(um1.getPayload().find((const uint8_t*)"F9") != string::npos)
|
||||
version9=true;
|
||||
}
|
||||
|
||||
if(version9)
|
||||
cerr<<"Detected version U-Blox 9"<<endl;
|
||||
|
||||
cerr<<"Sending GNSS query"<<endl;
|
||||
msg = buildUbxMessage(0x06, 0x3e, {});
|
||||
|
|
@ -416,40 +423,42 @@ int main(int argc, char** argv)
|
|||
auto payload = um1.getPayload();
|
||||
cerr<<"GNSS status, got " << (int)payload[3]<<" rows:\n";
|
||||
for(uint8_t n = 0 ; n < payload[3]; ++n) {
|
||||
cerr<<"GNSSID "<<(int)payload[4+8*n]<<" enabled "<<(int)payload[8+8*n]<<" minTrk "<< (int)payload[5+8*n] <<" maxTrk "<<(int)payload[6+8*n]<<endl;
|
||||
cerr<<"GNSSID "<<(int)payload[4+8*n]<<" enabled "<<(int)payload[8+8*n]<<" minTrk "<< (int)payload[5+8*n] <<" maxTrk "<<(int)payload[6+8*n]<<" " << (int)payload[8+8*n]<<" " << (int)payload[9+8*n] << " " <<" " << (int)payload[10+8*n]<<" " << (int)payload[11+8*n]<<endl;
|
||||
}
|
||||
|
||||
if(waitForUBXAckNack(fd, 2, 0x06, 0x3e)) {
|
||||
cerr<<"Got ACK for our poll of GNSS settings"<<endl;
|
||||
}
|
||||
|
||||
// ver RO maxch cfgs
|
||||
msg = buildUbxMessage(0x06, 0x3e, {0x00, 0x00, 0xff, 0x06,
|
||||
// GPS min max res x1 x2 x3, x4
|
||||
0x00, 0x04, 0x08, 0, doGPS, 0x00, 0x01, 0x00,
|
||||
// SBAS min max rex x1 x2 x3 x4
|
||||
0x01, 0x03, 0x04, 0, doSBAS, 0x00, 0x01, 0x00,
|
||||
// BEI min max res x1 x2 x3, x4
|
||||
0x03, 0x04, 0x08, 0, doBeidou, 0x00, 0x01, 0x00,
|
||||
// ??? min max res x1 x2 x3, x4
|
||||
0x05, 0x04, 0x08, 0, 0, 0x00, 0x01, 0x00,
|
||||
if(!version9) {
|
||||
// ver RO maxch cfgs
|
||||
msg = buildUbxMessage(0x06, 0x3e, {0x00, 0x00, 0xff, 0x06,
|
||||
// GPS min max res x1 x2 x3, x4
|
||||
0x00, 0x04, 0x08, 0, doGPS, 0x00, 0x01, 0x00,
|
||||
// SBAS min max rex x1 x2 x3 x4
|
||||
0x01, 0x03, 0x04, 0, doSBAS, 0x00, 0x01, 0x00,
|
||||
// BEI min max res x1 x2 x3, x4
|
||||
0x03, 0x04, 0x08, 0, doBeidou, 0x00, 0x01, 0x00,
|
||||
// ??? min max res x1 x2 x3, x4
|
||||
0x05, 0x04, 0x08, 0, 0, 0x00, 0x01, 0x00,
|
||||
|
||||
// GAL min max res x1 x2 x3, x4
|
||||
0x02, 0x04, 0x08, 0, doGalileo, 0x00, 0x01, 0x00,
|
||||
// GLO min max res x1 x2 x3, x4
|
||||
0x06, 0x06, 0x08, 0, doGlonass, 0x00, 0x01, 0x00
|
||||
// GAL min max res x1 x2 x3, x4
|
||||
0x02, 0x04, 0x08, 0, doGalileo, 0x00, 0x01, 0x00,
|
||||
// GLO min max res x1 x2 x3, x4
|
||||
0x06, 0x06, 0x08, 0, doGlonass, 0x00, 0x01, 0x00
|
||||
|
||||
});
|
||||
});
|
||||
|
||||
cerr<<"Sending GNSS setting, GPS: "<<doGPS<<", Galileo: "<<doGalileo<<", BeiDou: "<<doBeidou<<", GLONASS: "<<doGlonass<<", SBAS: "<<doSBAS<<endl;
|
||||
writen2(fd, msg.c_str(), msg.size());
|
||||
cerr<<"Sending GNSS setting, GPS: "<<doGPS<<", Galileo: "<<doGalileo<<", BeiDou: "<<doBeidou<<", GLONASS: "<<doGlonass<<", SBAS: "<<doSBAS<<endl;
|
||||
writen2(fd, msg.c_str(), msg.size());
|
||||
|
||||
if(waitForUBXAckNack(fd, 2, 0x06, 0x3e))
|
||||
cerr<<"Got ack on GNSS setting"<<endl;
|
||||
else {
|
||||
cerr<<"Got nack on GNSS setting"<<endl;
|
||||
exit(-1);
|
||||
if(waitForUBXAckNack(fd, 2, 0x06, 0x3e))
|
||||
cerr<<"Got ack on GNSS setting"<<endl;
|
||||
else {
|
||||
cerr<<"Got nack on GNSS setting"<<endl;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
|
||||
if(doSBAS) {
|
||||
// "on" "*.*" ign
|
||||
msg = buildUbxMessage(0x06, 0x16, {0x01, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00});
|
||||
|
|
@ -495,16 +504,22 @@ int main(int argc, char** argv)
|
|||
enableUBXMessageUSB(fd, 0x02, 0x59);
|
||||
|
||||
cerr<<"Enabling UBX-RXM-RAWX"<<endl; // RF doppler
|
||||
enableUBXMessageUSB(fd, 0x02, 0x15);
|
||||
enableUBXMessageUSB(fd, 0x02, 0x15, 16);
|
||||
|
||||
cerr<<"Enabling UBX-RXM-SFRBX"<<endl; // raw navigation frames
|
||||
enableUBXMessageUSB(fd, 0x02, 0x13);
|
||||
|
||||
cerr<<"Enabling UBX-NAV-POSECEF"<<endl; // position
|
||||
enableUBXMessageUSB(fd, 0x01, 0x01, 8);
|
||||
|
||||
cerr<<"Enabling UBX-NAV-SAT"<<endl; // satellite reception details
|
||||
enableUBXMessageUSB(fd, 0x01, 0x35, 4);
|
||||
|
||||
if(version9) {
|
||||
cerr<<"Enabling UBX-NAV-SIG"<<endl; // satellite reception details
|
||||
enableUBXMessageUSB(fd, 0x01, 0x43, 8);
|
||||
}
|
||||
else {
|
||||
cerr<<"Enabling UBX-NAV-SAT"<<endl; // satellite reception details
|
||||
enableUBXMessageUSB(fd, 0x01, 0x35, 8);
|
||||
}
|
||||
|
||||
cerr<<"Enabling UBX-NAV-PVT"<<endl; // position, velocity, time fix
|
||||
enableUBXMessageUSB(fd, 0x01, 0x07, 1);
|
||||
|
|
@ -586,7 +601,7 @@ int main(int argc, char** argv)
|
|||
// fmt::fprintf(stderr, "Our UTC : %02d:%02d:%06.4f -> %.4f or %d:%f -> delta = %.4fs\n", tm.tm_hour, tm.tm_min, seconds, ourutc, timestamp.tv_sec, 1.0*timestamp.tv_usec, ourutc - satutc);
|
||||
}
|
||||
}
|
||||
else if(msg.getClass() == 0x02 && msg.getType() == 0x15) { // RAWX
|
||||
else if(msg.getClass() == 0x02 && msg.getType() == 0x15) { // RAWX, the doppler stuff
|
||||
// cerr<<"Got "<<(int)payload[11] <<" measurements "<<endl;
|
||||
double rcvTow;
|
||||
memcpy(&rcvTow, &payload[0], 8);
|
||||
|
|
@ -599,9 +614,21 @@ int main(int argc, char** argv)
|
|||
memcpy(&prMes, &payload[16+32*n], 8);
|
||||
memcpy(&cpMes, &payload[24+32*n], 8);
|
||||
memcpy(&doppler, &payload[32+32*n], 4);
|
||||
|
||||
int gnssid = payload[36+32*n];
|
||||
int sv = payload[37+32*n];
|
||||
int sigid=0;
|
||||
if(version9) {
|
||||
sigid = payload[38+32*n];
|
||||
if(gnssid == 2 && sigid ==6) // they separate out I and Q, but the rest of UBX doesn't
|
||||
sigid = 5; // so map it back
|
||||
if(gnssid == 2 && sigid ==0) // they separate out I and Q, but the rest of UBX doesn't
|
||||
sigid = 1; // so map it back
|
||||
}
|
||||
else if(gnssid==2) { // version 8 defaults galileo to E1B
|
||||
sigid = 1;
|
||||
}
|
||||
|
||||
|
||||
uint16_t locktimems;
|
||||
memcpy(&locktimems, &payload[40+32*n], 2);
|
||||
uint8_t prStddev = payload[43+23*n] & 0xf;
|
||||
|
|
@ -617,6 +644,7 @@ int main(int argc, char** argv)
|
|||
|
||||
nmm.mutable_rfd()->set_gnssid(gnssid);
|
||||
nmm.mutable_rfd()->set_gnsssv(sv);
|
||||
nmm.mutable_rfd()->set_sigid(sigid);
|
||||
nmm.mutable_rfd()->set_rcvtow(rcvTow);
|
||||
nmm.mutable_rfd()->set_rcvwn(rcvWn);
|
||||
nmm.mutable_rfd()->set_doppler(doppler);
|
||||
|
|
@ -655,7 +683,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_op()->set_x(p.ecefX /100.0);
|
||||
nmm.mutable_op()->set_y(p.ecefY /100.0);
|
||||
nmm.mutable_op()->set_z(p.ecefZ /100.0);
|
||||
nmm.mutable_op()->set_acccm(p.pAcc /100.0);
|
||||
nmm.mutable_op()->set_acc(p.pAcc /100.0);
|
||||
emitNMM(1, nmm);
|
||||
}
|
||||
else if(msg.getClass() == 2 && msg.getType() == 0x13) { // SFRBX
|
||||
|
|
@ -666,21 +694,22 @@ int main(int argc, char** argv)
|
|||
|
||||
try {
|
||||
pair<int,int> id = make_pair(payload[0], payload[1]);
|
||||
static set<pair<int,int>> svseen;
|
||||
int sigid = payload[2];
|
||||
static set<tuple<int,int,int>> svseen;
|
||||
static time_t lastStat;
|
||||
svseen.insert(id);
|
||||
svseen.insert({id.first, id.second, payload[2]});
|
||||
|
||||
if(time(0)- lastStat > 30) {
|
||||
cerr<<"src "<<g_srcid<< " currently receiving: ";
|
||||
for(auto& s : svseen) {
|
||||
cerr<<s.first<<","<<s.second<<" ";
|
||||
cerr<<get<0>(s)<<","<<get<1>(s)<<"@"<<get<2>(s)<<" ";
|
||||
}
|
||||
cerr<<endl;
|
||||
lastStat = time(0);
|
||||
svseen.clear();
|
||||
}
|
||||
|
||||
if(id.first == 0) {
|
||||
if(id.first == 0 && !sigid) { // can only parse the old stuff
|
||||
NavMonMessage nmm;
|
||||
nmm.set_type(NavMonMessage::GPSInavType);
|
||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||
|
|
@ -691,13 +720,14 @@ int main(int argc, char** argv)
|
|||
auto gpsframe = getGPSFromSFRBXMsg(payload);
|
||||
auto cond = getCondensedGPSMessage(gpsframe);
|
||||
auto frameno = getbitu(&cond[0], 24+19, 3);
|
||||
if(frameno == 1)
|
||||
if(frameno == 1 && sigid==0)
|
||||
wn = 2048 + getbitu(&cond[0], 2*24, 10);
|
||||
if(!wn)
|
||||
continue; // can't file this yet
|
||||
|
||||
tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
|
||||
nmm.mutable_gpsi()->set_gnsswn(wn); // XXX this sucks
|
||||
nmm.mutable_gpsi()->set_sigid(sigid);
|
||||
nmm.mutable_gpsi()->set_gnsstow(tow); // "with 6 second increments" -- needs to be adjusted
|
||||
nmm.mutable_gpsi()->set_gnssid(id.first);
|
||||
nmm.mutable_gpsi()->set_gnsssv(id.second);
|
||||
|
|
@ -706,6 +736,8 @@ int main(int argc, char** argv)
|
|||
continue;
|
||||
}
|
||||
else if(id.first ==2) {
|
||||
// cerr<<"gal nav sv "<<id.second<<" size "<<payload.size();
|
||||
// cerr<<" res1 "<<(int)payload[2]<<" numwords "<<(int)payload[4] << " channel "<< (int)payload[5] << " version " << (int)payload[6]<<" res2 "<<(int)payload[7]<<endl;
|
||||
auto inav = getInavFromSFRBXMsg(payload);
|
||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||
tv[id] = timestamp;
|
||||
|
|
@ -754,9 +786,11 @@ int main(int argc, char** argv)
|
|||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||
|
||||
nmm.mutable_gi()->set_gnsswn(g_wn);
|
||||
|
||||
nmm.mutable_gi()->set_gnsstow(msgTOW);
|
||||
nmm.mutable_gi()->set_gnssid(id.first);
|
||||
nmm.mutable_gi()->set_gnsssv(id.second);
|
||||
nmm.mutable_gi()->set_sigid(sigid);
|
||||
nmm.mutable_gi()->set_contents((const char*)&inav[0], inav.size());
|
||||
|
||||
emitNMM(1, nmm);
|
||||
|
|
@ -771,7 +805,7 @@ int main(int argc, char** argv)
|
|||
bm.parse(cond, &pageno);
|
||||
|
||||
if(bm.wn < 0) {
|
||||
cerr<<"BeiDou C"<<id.second<<" WN not yetknown, not yet emitting message"<<endl;
|
||||
cerr<<"BeiDou C"<<id.second<<" WN not yet known, not yet emitting message"<<endl;
|
||||
continue;
|
||||
}
|
||||
NavMonMessage nmm;
|
||||
|
|
@ -785,6 +819,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_bid1()->set_gnsstow(bm.sow);
|
||||
nmm.mutable_bid1()->set_gnssid(id.first);
|
||||
nmm.mutable_bid1()->set_gnsssv(id.second);
|
||||
nmm.mutable_bid1()->set_sigid(sigid);
|
||||
nmm.mutable_bid1()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
}
|
||||
else {
|
||||
|
|
@ -793,6 +828,7 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_bid2()->set_gnsstow(bm.sow);
|
||||
nmm.mutable_bid2()->set_gnssid(id.first);
|
||||
nmm.mutable_bid2()->set_gnsssv(id.second);
|
||||
nmm.mutable_bid2()->set_sigid(sigid);
|
||||
nmm.mutable_bid2()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
}
|
||||
emitNMM(1, nmm);
|
||||
|
|
@ -815,12 +851,14 @@ int main(int argc, char** argv)
|
|||
nmm.mutable_gloi()->set_freq(payload[3]);
|
||||
nmm.mutable_gloi()->set_gnssid(id.first);
|
||||
nmm.mutable_gloi()->set_gnsssv(id.second);
|
||||
nmm.mutable_gloi()->set_sigid(sigid);
|
||||
nmm.mutable_gloi()->set_contents(string((char*)gstr.c_str(), gstr.size()));
|
||||
|
||||
emitNMM(1, nmm);
|
||||
}
|
||||
}
|
||||
else
|
||||
cerr<<"SFRBX from unsupported GNSSID "<<id.first<<", sv "<<id.second<<", "<<payload.size()<<" bytes"<<endl;
|
||||
cerr<<"SFRBX from unsupported GNSSID/sigid combination "<<id.first<<", sv "<<id.second<<", sigid "<<sigid<<", "<<payload.size()<<" bytes"<<endl;
|
||||
|
||||
#if 0
|
||||
if(0 && lasttv.count(id)) {
|
||||
|
|
@ -837,6 +875,8 @@ int main(int argc, char** argv)
|
|||
}
|
||||
}
|
||||
else if(msg.getClass() == 1 && msg.getType() == 0x35) { // UBX-NAV-SAT
|
||||
// if(version9) // we have UBX-NAV-SIG
|
||||
// continue;
|
||||
// cerr<< "Info for "<<(int) payload[5]<<" svs: \n";
|
||||
for(unsigned int n = 0 ; n < payload[5]; ++n) {
|
||||
int gnssid = payload[8+12*n];
|
||||
|
|
@ -861,6 +901,43 @@ int main(int argc, char** argv)
|
|||
emitNMM(1, nmm);
|
||||
}
|
||||
}
|
||||
else if(msg.getClass() == 1 && msg.getType() == 0x43) { // UBX-NAV-SIG
|
||||
for(unsigned int n = 0 ; n < payload[5]; ++n) {
|
||||
int gnssid = payload[8+16*n];
|
||||
int sv = payload[9+16*n];
|
||||
int sigid = 0;
|
||||
|
||||
if(version9) {
|
||||
sigid = payload[10+16*n];
|
||||
if(gnssid == 2 && sigid ==6) // they separate out I and Q, but the rest of UBX doesn't
|
||||
sigid = 5; // so map it back
|
||||
if(gnssid == 2 && sigid ==0) // they separate out I and Q, but the rest of UBX doesn't
|
||||
sigid = 1; // so map it back
|
||||
}
|
||||
else if(gnssid==2) { // version 8 defaults galileo to E1B
|
||||
sigid = 1;
|
||||
}
|
||||
|
||||
auto db = (int)payload[14+16*n];
|
||||
// cerr <<"gnssid "<<gnssid<<" sv "<<sv<<" el "<<el<<endl;
|
||||
NavMonMessage nmm;
|
||||
nmm.set_sourceid(g_srcid);
|
||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||
|
||||
nmm.set_type(NavMonMessage::ReceptionDataType);
|
||||
nmm.mutable_rd()->set_gnssid(gnssid);
|
||||
nmm.mutable_rd()->set_gnsssv(sv);
|
||||
nmm.mutable_rd()->set_db(db);
|
||||
nmm.mutable_rd()->set_prres(*((int16_t*)(payload.c_str()+ 12 +16*n)) *0.1); // ENDIANISM
|
||||
nmm.mutable_rd()->set_sigid(sigid);
|
||||
nmm.mutable_rd()->set_el(0);
|
||||
nmm.mutable_rd()->set_azi(0);
|
||||
|
||||
emitNMM(1, nmm);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// writen2(1, payload.d_raw.c_str(),msg.d_raw.size());
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue