move to new unified processing model, plus hook up sbas
bert hubert
parent
62c06629ff
commit
63827d8195
6
Makefile
6
Makefile
|
|
@ -1,6 +1,6 @@
|
|||
CFLAGS = -O3 -Wall -ggdb
|
||||
|
||||
CXXFLAGS:= -std=gnu++17 -Wall -O2 -MMD -MP -fno-omit-frame-pointer -Iext/CLI11 \
|
||||
CXXFLAGS:= -std=gnu++17 -Wall -O2 -ggdb -MMD -MP -fno-omit-frame-pointer -Iext/CLI11 \
|
||||
-Iext/fmt-6.1.2/include/ -Iext/powerblog/ext/simplesocket -Iext/powerblog/ext/ \
|
||||
-I/usr/local/opt/openssl/include/ \
|
||||
-Iext/sgp4/libsgp4/ \
|
||||
|
|
@ -36,7 +36,7 @@ clean:
|
|||
decrypt: decrypt.o bits.o ext/fmt-6.1.2/src/format.o
|
||||
$(CXX) -std=gnu++17 $^ -o $@
|
||||
|
||||
navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o
|
||||
navparse: navparse.o ext/fmt-6.1.2/src/format.o $(H2OPP) $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o trkmeas.o influxpush.o ${EXTRADEP} githash.o sbas.o
|
||||
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -L/usr/local/opt/openssl/lib/ -lh2o-evloop -lssl -lcrypto -lz -lcurl -lprotobuf $(WSLAY)
|
||||
|
||||
reporter: reporter.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ubx.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o navmon.o coverage.o osen.o githash.o
|
||||
|
|
@ -46,7 +46,7 @@ galmonmon: galmonmon.o ext/fmt-6.1.2/src/format.o $(SIMPLESOCKETS) minicurl.o ub
|
|||
$(CXX) -std=gnu++17 $^ -o $@ -pthread -L/usr/local/lib -lprotobuf -lcurl
|
||||
|
||||
|
||||
navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o ${EXTRADEP}
|
||||
navdump: navdump.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o navmon.o $(patsubst %.cc,%.o,$(wildcard ext/sgp4/libsgp4/*.cc)) tle.o sp3.o osen.o trkmeas.o githash.o rinex.o sbas.o ${EXTRADEP}
|
||||
$(CXX) -std=gnu++17 $^ -o $@ -L/usr/local/lib -pthread -lprotobuf -lz
|
||||
|
||||
navdisplay: navdisplay.o ext/fmt-6.1.2/src/format.o bits.o navmon.pb.o gps.o ephemeris.o beidou.o glonass.o ephemeris.o navmon.o osen.o githash.o
|
||||
|
|
|
|||
|
|
@ -17,7 +17,7 @@ int beidouBitconv(int their);
|
|||
C05 (58.75E)
|
||||
*/
|
||||
|
||||
struct BeidouMessage
|
||||
struct BeidouMessage : GPSLikeEphemeris
|
||||
{
|
||||
uint8_t strtype;
|
||||
|
||||
|
|
@ -118,7 +118,11 @@ struct BeidouMessage
|
|||
double getCrc() const { return ldexp(crc, -6); } // meters
|
||||
double getCrs() const { return ldexp(crs, -6); } // meters
|
||||
double getM0() const { return ldexp(m0 * M_PI, -31); } // radians
|
||||
|
||||
|
||||
int getIOD() const
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
|
||||
void parse2(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
|
|
|
|||
31
ephemeris.hh
31
ephemeris.hh
|
|
@ -2,6 +2,37 @@
|
|||
#include "minivec.hh"
|
||||
#include <iostream>
|
||||
#include <tuple>
|
||||
#include <stdint.h>
|
||||
struct GPSLikeEphemeris
|
||||
{
|
||||
virtual double getMu() const = 0;
|
||||
virtual double getOmegaE() const = 0;
|
||||
virtual double getE() const = 0;
|
||||
virtual uint32_t getT0e() const = 0;
|
||||
|
||||
virtual double getI0() const = 0;
|
||||
|
||||
virtual double getOmegadot() const = 0;
|
||||
|
||||
virtual double getSqrtA() const = 0;
|
||||
virtual double getOmega0() const = 0;
|
||||
virtual double getOmega() const = 0;
|
||||
|
||||
virtual double getM0() const = 0;
|
||||
virtual double getIdot() const = 0;
|
||||
virtual double getCic() const = 0;
|
||||
virtual double getCis() const = 0;
|
||||
virtual double getCuc() const = 0;
|
||||
virtual double getCus() const = 0;
|
||||
virtual double getCrc() const = 0;
|
||||
virtual double getCrs() const = 0;
|
||||
virtual double getDeltan()const = 0;
|
||||
|
||||
virtual int getIOD() const = 0;
|
||||
// maybe af0, af1, af2?
|
||||
// maybe getUTCOffset? getAtomicOffset etc
|
||||
|
||||
};
|
||||
|
||||
// lat, lon, height (rad, rad, meters)
|
||||
std::tuple<double, double, double> ecefToWGS84(double x, double y, double z);
|
||||
|
|
|
|||
18
galileo.hh
18
galileo.hh
|
|
@ -8,7 +8,7 @@
|
|||
|
||||
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn);
|
||||
|
||||
struct GalileoMessage
|
||||
struct GalileoMessage : GPSLikeEphemeris
|
||||
{
|
||||
uint8_t wtype;
|
||||
|
||||
|
|
@ -41,8 +41,8 @@ struct GalileoMessage
|
|||
}
|
||||
|
||||
uint8_t sparetime{0};
|
||||
uint16_t wn{0}; // we put the "unrolled" week number here!
|
||||
uint32_t tow{0}; // "last seen"
|
||||
uint16_t wn{0};
|
||||
uint32_t tow{0};
|
||||
int iodalmanac;
|
||||
int wnalmanac;
|
||||
int t0almanac;
|
||||
|
|
@ -99,6 +99,11 @@ struct GalileoMessage
|
|||
|
||||
uint16_t iodnav;
|
||||
|
||||
int getIOD() const
|
||||
{
|
||||
return iodnav;
|
||||
}
|
||||
|
||||
struct Almanac
|
||||
{
|
||||
int svid{-1};
|
||||
|
|
@ -131,7 +136,10 @@ struct GalileoMessage
|
|||
double getCrc() const { return 0; } // meters
|
||||
double getCrs() const { return 0; } // meters
|
||||
double getDeltan()const { return 0; } //radians/s
|
||||
|
||||
int getIOD() const
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
} alma1, alma2, alma3;
|
||||
|
||||
|
||||
|
|
@ -211,7 +219,7 @@ struct GalileoMessage
|
|||
|
||||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
// pair of nanosecond, nanosecond/s
|
||||
std::pair<double, double> getGPSOffset(int tow, int wn) const
|
||||
{
|
||||
int dw = (int)(uint8_t)wn - (int)(uint8_t) wn0g;
|
||||
|
|
|
|||
40
galmonmon.cc
40
galmonmon.cc
|
|
@ -166,7 +166,7 @@ std::optional<string> StateKeeper::reportState(string_view thing, string_view na
|
|||
|
||||
|
||||
StateKeeper g_sk;
|
||||
|
||||
#if 0
|
||||
static std::string string_replace(const std::string& str, const std::string& match,
|
||||
const std::string& replacement, unsigned int max_replacements = UINT_MAX)
|
||||
{
|
||||
|
|
@ -182,12 +182,14 @@ static std::string string_replace(const std::string& str, const std::string& mat
|
|||
}
|
||||
return newstr;
|
||||
}
|
||||
|
||||
#endif
|
||||
void sendTweet(const string& tweet)
|
||||
{
|
||||
string etweet = tweet;
|
||||
//system((string("twurl -X POST /1.1/statuses/update.json -d \"media_ids=1215649475231997953&status=")+etweet+"\" >> twitter.log").c_str());
|
||||
system((string("twurl -X POST /1.1/statuses/update.json -d \"status=")+etweet+"\" >> twitter.log").c_str());
|
||||
if(system((string("twurl -X POST /1.1/statuses/update.json -d \"status=")+etweet+"\" >> twitter.log").c_str()) < 0) {
|
||||
cout<<"Problem tweeting!"<<endl;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -247,7 +249,27 @@ int main(int argc, char **argv)
|
|||
cout<<"Galmon behind by " << (time(0) - (long) iter.value()) <<" seconds"<<endl;
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
res = mc.getURL(url+"sbas.json");
|
||||
j = nlohmann::json::parse(res);
|
||||
std::optional<string> sbasHealthChange;
|
||||
for(auto iter = j.begin(); iter != j.end(); ++iter) {
|
||||
if(iter.value().count("health")) {
|
||||
string name = sbasName(atoi(iter.key().c_str()));
|
||||
sbasHealthChange = g_sk.reportState(name, "sbas-health", (string)iter.value()["health"]);
|
||||
// cout<<"Setting state for "<< name <<" to "<< (string)iter.value()["health"] << endl;
|
||||
|
||||
|
||||
if(sbasHealthChange) {
|
||||
ostringstream out;
|
||||
out<<"✈️ augmentation system "<<name<<" health changed: "<<*sbasHealthChange;
|
||||
cout<<out.str()<<endl;
|
||||
if(doTweet)
|
||||
sendTweet(out.str());
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
res = mc.getURL(url+"svs.json");
|
||||
j = nlohmann::json::parse(res);
|
||||
|
|
@ -295,7 +317,7 @@ int main(int argc, char **argv)
|
|||
|
||||
auto seenChange = g_sk.reportState(fullName, "silent", notseen);
|
||||
|
||||
auto sisaChange = g_sk.reportState(fullName, "sisa", (string)sv["sisa"]);
|
||||
auto sisaChange = g_sk.reportState(fullName, "sisa", (double) sv["sisa-m"], (string)sv["sisa"]);
|
||||
|
||||
double ephdisco = sv.count("latest-disco") ? (double)sv["latest-disco"] : -1.0;
|
||||
auto ephdiscochange = g_sk.reportState(fullName, "eph-disco", ephdisco);
|
||||
|
|
@ -357,9 +379,13 @@ int main(int argc, char **argv)
|
|||
|
||||
if(sisaChange) {
|
||||
ostringstream tmp;
|
||||
tmp<< " SISA/URA reported ranging accuracy changed, new: "<<*sisaChange<<", old: " << *g_sk.getPrevState(fullName, "sisa");
|
||||
if(tmp.str().find("200 cm") == string::npos || tmp.str().find("282 cm") == string::npos)
|
||||
auto state = g_sk.getFullState(fullName, "sisa");
|
||||
auto prevState = g_sk.getPrevFullState(fullName, "sisa");
|
||||
tmp<< " SISA/URA reported ranging accuracy changed, new: "<<state->text<<", old: " << prevState->text;
|
||||
if(get<double>(state->state) > 3 || get<double>(prevState->state) > 3)
|
||||
out << tmp.str();
|
||||
else
|
||||
cout<<"Not reporting: "<<tmp.str()<<endl;
|
||||
}
|
||||
|
||||
string tweet;
|
||||
|
|
|
|||
23
glonass.cc
23
glonass.cc
|
|
@ -1,6 +1,10 @@
|
|||
#include "glonass.hh"
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
using std::cout;
|
||||
using std::endl;
|
||||
|
||||
static const double ae = 6378.136; // km // IERS: 6378136.6
|
||||
static const double mu = 398.6004418E3; // km3/s2 // IERS: 3.986004418
|
||||
|
|
@ -98,8 +102,25 @@ static void rk4step (const double A[3], double y[6], double h)
|
|||
y[j] = y[j] + h * (k1[j] + 2*(k2[j] + k3[j]) + k4[j]) / 6;
|
||||
}
|
||||
|
||||
|
||||
using Clock = std::chrono::steady_clock;
|
||||
|
||||
static double passedMsec(const Clock::time_point& then, const Clock::time_point& now)
|
||||
{
|
||||
return std::chrono::duration_cast<std::chrono::microseconds>(now - then).count()/1000.0;
|
||||
}
|
||||
|
||||
|
||||
static double passedMsec(const Clock::time_point& then)
|
||||
{
|
||||
return passedMsec(then, Clock::now());
|
||||
}
|
||||
|
||||
|
||||
double getCoordinates(double tow, const GlonassMessage& eph, Point* p)
|
||||
{
|
||||
auto start = Clock::now();
|
||||
|
||||
double y0[6] = {ldexp(eph.x, -11), ldexp(eph.y, -11), ldexp(eph.z, -11),
|
||||
ldexp(eph.dx, -20), ldexp(eph.dy, -20), ldexp(eph.dz, -20)};
|
||||
double A[3] = {ldexp(eph.ddx, -30), ldexp(eph.ddy, -30), ldexp(eph.ddz, -30)};
|
||||
|
|
@ -116,5 +137,7 @@ double getCoordinates(double tow, const GlonassMessage& eph, Point* p)
|
|||
rk4step (A, y0, h);
|
||||
|
||||
*p = Point (1E3*y0[0], 1E3*y0[1], 1E3*y0 [2]);
|
||||
static double total=0;
|
||||
// cout<<"Took: "<<(total+=passedMsec(start))<<" ms" <<endl;
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
24
glonass.hh
24
glonass.hh
|
|
@ -53,6 +53,9 @@ struct GlonassMessage
|
|||
double getdY() { return ldexp(dy*1000.0, -20); }
|
||||
double getdZ() { return ldexp(dz*1000.0, -20); }
|
||||
|
||||
// this is there to make doDoppler work, which sadly wants to do
|
||||
// arithmetic to get the age of an ephemeris
|
||||
double getT0e() const { return 0; }
|
||||
|
||||
double getRadius() { return sqrt(getX()*getX() + getY()*getY() + getZ()*getZ()); }
|
||||
|
||||
|
|
@ -127,7 +130,26 @@ struct GlonassMessage
|
|||
P4 = getbitu(&gstr[0], 85-34, 1);
|
||||
deltaTaun = getbitsglonass(&gstr[0], 85 - 58, 4);
|
||||
}
|
||||
// nanosecond, nanosecond/s pair
|
||||
std::pair<double, double> getUTCOffset(int tow) const
|
||||
{
|
||||
std::pair<double, double> ret;
|
||||
ret.second=0;
|
||||
|
||||
ret.first = 1000000000.0*ldexp(tauc, -31); // this is Glonass-M
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
std::pair<double, double> getGPSOffset(int tow) const
|
||||
{
|
||||
std::pair<double, double> ret;
|
||||
ret.second=0;
|
||||
ret.first = 1000000000.0*ldexp(taugps, -30);
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
uint32_t getGloTime() const;
|
||||
|
||||
uint8_t n4{0}; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in
|
||||
|
|
@ -138,7 +160,7 @@ struct GlonassMessage
|
|||
{
|
||||
n4=getbitu(&gstr[0], 85-36, 5);
|
||||
taugps = getbitsglonass(&gstr[0], 85-31, 22);
|
||||
tauc = getbitsglonass(&gstr[0], 85-69, 32);
|
||||
tauc = getbitsglonass(&gstr[0], 85-69, 32); // check the NEW ICD
|
||||
l_n = getbitu(&gstr[0], 85 - 9, 1);
|
||||
}
|
||||
|
||||
|
|
|
|||
119
gps.cc
119
gps.cc
|
|
@ -13,3 +13,122 @@ std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t
|
|||
|
||||
}
|
||||
|
||||
// expects input as 24 bit read to to use messages, returns frame number
|
||||
int GPSState::parseGPSMessage(std::basic_string_view<uint8_t> cond, uint8_t* pageptr)
|
||||
{
|
||||
using namespace std;
|
||||
int frame = getbitu(&cond[0], 24+19, 3);
|
||||
// 10 * 4 bytes in payload now
|
||||
tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
|
||||
// cerr << "Preamble: "<<getbitu(&cond[0], 0, 8) <<", frame: "<< frame<<", truncated TOW: "<<tow<<endl;
|
||||
if(frame == 1) {
|
||||
// word 1, word 2 are TLM and HOW
|
||||
// 2 bits of padding on each word
|
||||
// word3:
|
||||
// 1-10: WN
|
||||
// 11-12: Which codes 00 = invalid, 01 = P-code on, 10 = C/A code on, 11 invalid
|
||||
// 13-16: URA, 0-15 scale
|
||||
// 17-22: 0 is ok
|
||||
// 23-24: MSB of IODC
|
||||
|
||||
// word 8:
|
||||
// 1-8: LSB of IODC
|
||||
// 9-24:
|
||||
|
||||
wn = 2048 + getbitu(&cond[0], 2*24, 10);
|
||||
ura = getbitu(&cond[0], 2*24+12, 4);
|
||||
gpshealth = getbitu(&cond[0], 2*24+16, 6);
|
||||
|
||||
// cerr<<"GPS Week Number: "<< wn <<", URA: "<< (int)ura<<", health: "<<
|
||||
// (int)gpshealth <<endl;
|
||||
|
||||
af2 = getbits(&cond[0], 8*24, 8); // * 2^-55
|
||||
af1 = getbits(&cond[0], 8*24 + 8, 16); // * 2^-43
|
||||
af0 = getbits(&cond[0], 9*24, 22); // * 2^-31
|
||||
t0c = getbitu(&cond[0], 7*24 + 8, 16); // * 16
|
||||
// cerr<<"t0c*16: "<<t0c*16<<", af2: "<< (int)af2 <<", af1: "<< af1 <<", af0: "<<
|
||||
// af0 <<endl;
|
||||
}
|
||||
else if(frame == 2) {
|
||||
gpsiod = getbitu(&cond[0], 2*24, 8);
|
||||
t0e = getbitu(&cond[0], 9*24, 16) * 16.0; // WE SCALE THIS FOR THE USER!!
|
||||
// cerr<<"IODe "<<(int)iod<<", t0e "<< t0e << " = "<< 16* t0e <<"s"<<endl;
|
||||
|
||||
e= getbitu(&cond[0], 5*24+16, 32);
|
||||
// cerr<<"e: "<<ldexp(e, -33)<<", ";
|
||||
|
||||
|
||||
// sqrt(A), 32 bits, 2^-19
|
||||
sqrtA= getbitu(&cond[0], 7*24+ 16, 32);
|
||||
// double sqrtA=ldexp(sqrtA, -19); // 2^-19
|
||||
// cerr<<"Radius: "<<sqrtA*sqrtA<<endl;
|
||||
|
||||
crs = getbits(&cond[0], 2*24 + 8, 16); // 2^-5 meters
|
||||
deltan = getbits(&cond[0], 3*24, 16); // 2^-43 semi-circles/s
|
||||
m0 = getbits(&cond[0], 3*24+16, 32); // 2^-31 semi-circles
|
||||
|
||||
cuc = getbits(&cond[0], 5*24, 16); // 2^-29 RADIANS
|
||||
cus = getbits(&cond[0], 7*24, 16); // 2^-29 RADIANS
|
||||
}
|
||||
else if(frame == 3) {
|
||||
gpsiod = getbitu(&cond[0], 9*24, 8);
|
||||
cic = getbits(&cond[0], 2*24, 16); // 2^-29 RADIANS
|
||||
omega0 = getbits(&cond[0], 2*24 + 16, 32); // 2^-31 semi-circles
|
||||
cis = getbits(&cond[0], 4*24, 16); // 2^-29 radians
|
||||
i0 = getbits(&cond[0], 4*24 + 16, 32); // 2^-31, semicircles
|
||||
|
||||
crc = getbits(&cond[0], 6*24, 16); // 2^-5, meters
|
||||
omega = getbits(&cond[0], 6*24+16, 32); // 2^-31, semi-circles
|
||||
|
||||
omegadot = getbits(&cond[0], 8*24, 24); // 2^-43, semi-circles/s
|
||||
idot = getbits(&cond[0], 9*24+8, 14); // 2^-43, semi-cirlces/s
|
||||
}
|
||||
else if(frame == 4) { // this is a carousel frame
|
||||
int page = getbitu(&cond[0], 2*24 + 2, 6);
|
||||
if(pageptr)
|
||||
*pageptr=0;
|
||||
// cerr<<"Frame 4, page "<<page;
|
||||
if(page == 56) { // 56 is the new 18 somehow? See table 20-V of the ICD
|
||||
if(pageptr)
|
||||
*pageptr=18;
|
||||
a0 = getbits(&cond[0], 6*24 , 32); // 2^-30
|
||||
a1 = getbits(&cond[0], 5*24 , 24); // 2^-50
|
||||
|
||||
t0t = getbitu(&cond[0], 7*24 + 8, 8) * 4096; // WE SCALE THIS FOR THE USER!
|
||||
wn0t = getbitu(&cond[0], 7*24 + 16, 8);
|
||||
dtLS = getbits(&cond[0], 8*24, 8);
|
||||
dtLSF = getbits(&cond[0], 9*24, 8);
|
||||
|
||||
// cerr<<": a0: "<<a0<<", a1: "<<a1<<", t0t: "<< t0t * (1<<12) <<", wn0t: "<< wn0t<<", rough offset: "<<ldexp(a0, -30)<<endl;
|
||||
// cerr<<"deltaTLS: "<< (int)dtLS<<", post "<< (int)dtLSF<<endl;
|
||||
return frame; // otherwise pageptr gets overwritten below
|
||||
}
|
||||
//else cerr<<endl;
|
||||
// page 18 contains UTC -> 56
|
||||
// page 25 -> 63
|
||||
// 2-10 -> 25 -> 32 ??
|
||||
}
|
||||
|
||||
if(frame == 5 || frame==4) { // this is a caroussel frame
|
||||
gpsalma.dataid = getbitu(&cond[0], 2*24, 2);
|
||||
gpsalma.sv = getbitu(&cond[0], 2*24+2, 6);
|
||||
|
||||
if(pageptr)
|
||||
*pageptr= gpsalma.sv;
|
||||
|
||||
|
||||
gpsalma.e = getbitu(&cond[0], 2*24 + 8, 16);
|
||||
gpsalma.t0a = getbitu(&cond[0], 3*24, 8);
|
||||
gpsalma.deltai = getbits(&cond[0], 3*24 +8 , 16);
|
||||
gpsalma.omegadot = getbits(&cond[0], 4*24, 16);
|
||||
gpsalma.health = getbitu(&cond[0], 4*24 +16, 8);
|
||||
gpsalma.sqrtA = getbitu(&cond[0], 5*24, 24);
|
||||
gpsalma.Omega0 = getbits(&cond[0], 6*24, 24);
|
||||
gpsalma.omega = getbits(&cond[0], 7*24, 24);
|
||||
gpsalma.M0 = getbits(&cond[0], 8*24, 24);
|
||||
gpsalma.af0 = (getbits(&cond[0], 9*24, 8) << 3) + getbits(&cond[0], 9*24 +19, 3);
|
||||
gpsalma.af1 = getbits(&cond[0], 9*24 + 8, 11);
|
||||
// cerr<<"Frame 5, SV: "<<getbitu(&cond[0], 2*32 + 2 +2, 6)<<endl;
|
||||
}
|
||||
return frame;
|
||||
}
|
||||
|
|
|
|||
191
gps.hh
191
gps.hh
|
|
@ -5,9 +5,12 @@
|
|||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
#include "ephemeris.hh"
|
||||
|
||||
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
struct GPSAlmanac
|
||||
|
||||
struct GPSAlmanac : GPSLikeEphemeris
|
||||
{
|
||||
int dataid{-1};
|
||||
int sv;
|
||||
|
|
@ -29,7 +32,7 @@ struct GPSAlmanac
|
|||
{
|
||||
return ldexp(e, -21);
|
||||
}
|
||||
double getT0e() const
|
||||
uint32_t getT0e() const
|
||||
{
|
||||
return ldexp(t0a, 12);
|
||||
}
|
||||
|
|
@ -71,30 +74,23 @@ struct GPSAlmanac
|
|||
double getCrs() const { return 0; } // meters
|
||||
double getDeltan()const { return 0; } //radians/s
|
||||
|
||||
|
||||
int getIOD() const { return 0; } // XXX ioda?
|
||||
};
|
||||
|
||||
|
||||
struct GPSState
|
||||
struct GPSState : GPSLikeEphemeris
|
||||
{
|
||||
struct SVIOD
|
||||
{
|
||||
std::bitset<32> words;
|
||||
int gnssid;
|
||||
uint32_t t0e;
|
||||
uint32_t e, sqrtA;
|
||||
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
|
||||
|
||||
uint32_t t0e;
|
||||
uint32_t e, sqrtA;
|
||||
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
|
||||
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
|
||||
// 16 seconds
|
||||
uint16_t t0c;
|
||||
|
||||
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
|
||||
// 16 seconds
|
||||
uint16_t t0c;
|
||||
|
||||
// 2^-31 2^-43
|
||||
int32_t af0 , af1;
|
||||
// ???
|
||||
int8_t af2;
|
||||
uint32_t wn{0}, tow{0};
|
||||
// 2^-31 2^-43
|
||||
int32_t af0 , af1;
|
||||
// ???
|
||||
int8_t af2;
|
||||
|
||||
double getMu() const
|
||||
{
|
||||
|
|
@ -120,17 +116,12 @@ struct GPSState
|
|||
double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s
|
||||
double getOmega() const { return ldexp(omega * M_PI, -31); } // radians
|
||||
|
||||
|
||||
};
|
||||
|
||||
GPSAlmanac gpsalma;
|
||||
|
||||
uint8_t gpshealth{0};
|
||||
uint16_t ai0{0};
|
||||
int16_t ai1{0}, ai2{0};
|
||||
|
||||
int BGDE1E5a{0}, BGDE1E5b{0};
|
||||
|
||||
bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
|
||||
uint16_t wn{0}; // we put the "unrolled" week number here!
|
||||
uint32_t tow{0}; // "last seen"
|
||||
|
|
@ -142,27 +133,15 @@ struct GPSState
|
|||
uint16_t wnLSF{0};
|
||||
uint8_t dn; // leap second day number
|
||||
// 1 2^-31 2^-43 2^-55 16 second
|
||||
int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX
|
||||
int ura;
|
||||
|
||||
int gpsiod{-1};
|
||||
|
||||
std::map<int, SVIOD> iods;
|
||||
SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor
|
||||
void checkCompleteAndClean(int iod)
|
||||
|
||||
int getIOD() const
|
||||
{
|
||||
if(iods[iod].words[2] && iods[iod].words[3]) {
|
||||
if(iods.size() > 1) {
|
||||
auto tmp = iods[iod];
|
||||
iods.clear();
|
||||
iods[iod] = tmp;
|
||||
}
|
||||
}
|
||||
return gpsiod;
|
||||
}
|
||||
bool isComplete(int iod)
|
||||
{
|
||||
return iods[iod].words[2] && iods[iod].words[3];
|
||||
}
|
||||
|
||||
int parseGPSMessage(std::basic_string_view<uint8_t> cond, uint8_t* pageptr=0);
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
|
|
@ -203,129 +182,3 @@ std::pair<double, double> getGPSUTCOffset(int tow, int wn, const T& eph)
|
|||
|
||||
|
||||
|
||||
// expects input as 24 bit read to to use messages, returns frame number
|
||||
template<typename T>
|
||||
int parseGPSMessage(std::basic_string_view<uint8_t> cond, T& out, uint8_t* pageptr=0)
|
||||
{
|
||||
using namespace std;
|
||||
int frame = getbitu(&cond[0], 24+19, 3);
|
||||
// 10 * 4 bytes in payload now
|
||||
out.tow = 1.5*(getbitu(&cond[0], 24, 17)*4);
|
||||
// cerr << "Preamble: "<<getbitu(&cond[0], 0, 8) <<", frame: "<< frame<<", truncated TOW: "<<out.tow<<endl;
|
||||
if(frame == 1) {
|
||||
// word 1, word 2 are TLM and HOW
|
||||
// 2 bits of padding on each word
|
||||
// word3:
|
||||
// 1-10: WN
|
||||
// 11-12: Which codes 00 = invalid, 01 = P-code on, 10 = C/A code on, 11 invalid
|
||||
// 13-16: URA, 0-15 scale
|
||||
// 17-22: 0 is ok
|
||||
// 23-24: MSB of IODC
|
||||
|
||||
// word 8:
|
||||
// 1-8: LSB of IODC
|
||||
// 9-24:
|
||||
|
||||
out.wn = 2048 + getbitu(&cond[0], 2*24, 10);
|
||||
out.ura = getbitu(&cond[0], 2*24+12, 4);
|
||||
out.gpshealth = getbitu(&cond[0], 2*24+16, 6);
|
||||
|
||||
// cerr<<"GPS Week Number: "<< out.wn <<", URA: "<< (int)out.ura<<", health: "<<
|
||||
// (int)out.gpshealth <<endl;
|
||||
|
||||
out.af2 = getbits(&cond[0], 8*24, 8); // * 2^-55
|
||||
out.af1 = getbits(&cond[0], 8*24 + 8, 16); // * 2^-43
|
||||
out.af0 = getbits(&cond[0], 9*24, 22); // * 2^-31
|
||||
out.t0c = getbitu(&cond[0], 7*24 + 8, 16); // * 16
|
||||
// cerr<<"t0c*16: "<<out.t0c*16<<", af2: "<< (int)out.af2 <<", af1: "<< out.af1 <<", af0: "<<
|
||||
// out.af0 <<endl;
|
||||
}
|
||||
else if(frame == 2) {
|
||||
out.gpsiod = getbitu(&cond[0], 2*24, 8);
|
||||
auto& eph = out.getEph(out.gpsiod);
|
||||
eph.words[2]=1;
|
||||
eph.t0e = getbitu(&cond[0], 9*24, 16) * 16.0; // WE SCALE THIS FOR THE USER!!
|
||||
// cerr<<"IODe "<<(int)iod<<", t0e "<< eph.t0e << " = "<< 16* eph.t0e <<"s"<<endl;
|
||||
|
||||
eph.e= getbitu(&cond[0], 5*24+16, 32);
|
||||
// cerr<<"e: "<<ldexp(eph.e, -33)<<", ";
|
||||
|
||||
|
||||
// sqrt(A), 32 bits, 2^-19
|
||||
eph.sqrtA= getbitu(&cond[0], 7*24+ 16, 32);
|
||||
// double sqrtA=ldexp(eph.sqrtA, -19); // 2^-19
|
||||
// cerr<<"Radius: "<<sqrtA*sqrtA<<endl;
|
||||
|
||||
eph.crs = getbits(&cond[0], 2*24 + 8, 16); // 2^-5 meters
|
||||
eph.deltan = getbits(&cond[0], 3*24, 16); // 2^-43 semi-circles/s
|
||||
eph.m0 = getbits(&cond[0], 3*24+16, 32); // 2^-31 semi-circles
|
||||
|
||||
eph.cuc = getbits(&cond[0], 5*24, 16); // 2^-29 RADIANS
|
||||
eph.cus = getbits(&cond[0], 7*24, 16); // 2^-29 RADIANS
|
||||
out.checkCompleteAndClean(out.gpsiod);
|
||||
}
|
||||
else if(frame == 3) {
|
||||
out.gpsiod = getbitu(&cond[0], 9*24, 8);
|
||||
auto& eph = out.getEph(out.gpsiod);
|
||||
eph.words[3]=1;
|
||||
eph.cic = getbits(&cond[0], 2*24, 16); // 2^-29 RADIANS
|
||||
eph.omega0 = getbits(&cond[0], 2*24 + 16, 32); // 2^-31 semi-circles
|
||||
eph.cis = getbits(&cond[0], 4*24, 16); // 2^-29 radians
|
||||
eph.i0 = getbits(&cond[0], 4*24 + 16, 32); // 2^-31, semicircles
|
||||
|
||||
eph.crc = getbits(&cond[0], 6*24, 16); // 2^-5, meters
|
||||
eph.omega = getbits(&cond[0], 6*24+16, 32); // 2^-31, semi-circles
|
||||
|
||||
eph.omegadot = getbits(&cond[0], 8*24, 24); // 2^-43, semi-circles/s
|
||||
eph.idot = getbits(&cond[0], 9*24+8, 14); // 2^-43, semi-cirlces/s
|
||||
out.checkCompleteAndClean(out.gpsiod);
|
||||
}
|
||||
else if(frame == 4) { // this is a carousel frame
|
||||
int page = getbitu(&cond[0], 2*24 + 2, 6);
|
||||
if(pageptr)
|
||||
*pageptr=0;
|
||||
// cerr<<"Frame 4, page "<<page;
|
||||
if(page == 56) { // 56 is the new 18 somehow? See table 20-V of the ICD
|
||||
if(pageptr)
|
||||
*pageptr=18;
|
||||
out.a0 = getbits(&cond[0], 6*24 , 32); // 2^-30
|
||||
out.a1 = getbits(&cond[0], 5*24 , 24); // 2^-50
|
||||
|
||||
out.t0t = getbitu(&cond[0], 7*24 + 8, 8) * 4096; // WE SCALE THIS FOR THE USER!
|
||||
out.wn0t = getbitu(&cond[0], 7*24 + 16, 8);
|
||||
out.dtLS = getbits(&cond[0], 8*24, 8);
|
||||
out.dtLSF = getbits(&cond[0], 9*24, 8);
|
||||
|
||||
// cerr<<": a0: "<<out.a0<<", a1: "<<out.a1<<", t0t: "<< out.t0t * (1<<12) <<", wn0t: "<< out.wn0t<<", rough offset: "<<ldexp(out.a0, -30)<<endl;
|
||||
// cerr<<"deltaTLS: "<< (int)out.dtLS<<", post "<< (int)out.dtLSF<<endl;
|
||||
return frame; // otherwise pageptr gets overwritten below
|
||||
}
|
||||
//else cerr<<endl;
|
||||
// page 18 contains UTC -> 56
|
||||
// page 25 -> 63
|
||||
// 2-10 -> 25 -> 32 ??
|
||||
}
|
||||
|
||||
if(frame == 5 || frame==4) { // this is a caroussel frame
|
||||
out.gpsalma.dataid = getbitu(&cond[0], 2*24, 2);
|
||||
out.gpsalma.sv = getbitu(&cond[0], 2*24+2, 6);
|
||||
|
||||
if(pageptr)
|
||||
*pageptr= out.gpsalma.sv;
|
||||
|
||||
|
||||
out.gpsalma.e = getbitu(&cond[0], 2*24 + 8, 16);
|
||||
out.gpsalma.t0a = getbitu(&cond[0], 3*24, 8);
|
||||
out.gpsalma.deltai = getbits(&cond[0], 3*24 +8 , 16);
|
||||
out.gpsalma.omegadot = getbits(&cond[0], 4*24, 16);
|
||||
out.gpsalma.health = getbitu(&cond[0], 4*24 +16, 8);
|
||||
out.gpsalma.sqrtA = getbitu(&cond[0], 5*24, 24);
|
||||
out.gpsalma.Omega0 = getbits(&cond[0], 6*24, 24);
|
||||
out.gpsalma.omega = getbits(&cond[0], 7*24, 24);
|
||||
out.gpsalma.M0 = getbits(&cond[0], 8*24, 24);
|
||||
out.gpsalma.af0 = (getbits(&cond[0], 9*24, 8) << 3) + getbits(&cond[0], 9*24 +19, 3);
|
||||
out.gpsalma.af1 = getbits(&cond[0], 9*24 + 8, 11);
|
||||
// cerr<<"Frame 5, SV: "<<getbitu(&cond[0], 2*32 + 2 +2, 6)<<endl;
|
||||
}
|
||||
return frame;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -304,7 +304,16 @@ function update()
|
|||
|
||||
|
||||
d3.json("global.json", function(d) {
|
||||
d3.select('#facts').html("Galileo-UTC offset: <b>"+d["utc-offset-ns"].toFixed(2)+"</b> ns, Galileo-GPS offset: <b>"+d["gps-offset-ns"].toFixed(2)+"</b> ns, GPS UTC offset: <b>"+d["gps-utc-offset-ns"].toFixed(2)+"</b>. "+d["leap-seconds"]+"</b> leap seconds");
|
||||
console.log(d);
|
||||
var str="Galileo-UTC offset: <b>"+d["gst-utc-offset-ns"].toFixed(2)+"</b> ns";
|
||||
str += ", Galileo-GPS offset: <b>"+d["gst-gps-offset-ns"].toFixed(2)+"</b> ns";
|
||||
str += " , GPS-UTC offset: <b>"+d["gps-utc-offset-ns"].toFixed(2)+" ns</b>";
|
||||
str += " , BeiDou-UTC offset: <b>"+d["beidou-utc-offset-ns"].toFixed(2)+" ns</b>";
|
||||
str += " , GLONASS-UTC offset: <b>"+d["glonass-utc-offset-ns"].toFixed(2)+" ns</b>";
|
||||
str += " , GLONASS-GPS offset: <b>"+d["glonass-gps-offset-ns"].toFixed(2)+" ns</b>";
|
||||
str += ", GPS-UTC offset: <b>"+d["beidou-utc-offset-ns"].toFixed(2)+" ns</b>, "+d["leap-seconds"]+"</b> leap seconds";
|
||||
|
||||
d3.select('#facts').html(str);
|
||||
lastseen = moment(1000*d["last-seen"]);
|
||||
d3.select("#freshness").html(lastseen.fromNow());
|
||||
});
|
||||
|
|
|
|||
|
|
@ -137,6 +137,7 @@ function makeTable(str, obj)
|
|||
Object.keys(obj).forEach(function(e) {
|
||||
if(e=="svs") {
|
||||
Object.keys(obj[e]).forEach(function(k) {
|
||||
if(obj[e][k].elev && obj[e][k].azi) {
|
||||
var obj2 ={id: k, elev: obj[e][k].elev.toFixed(1),
|
||||
sigid: obj[e][k].sigid,
|
||||
db: obj[e][k].db, azi: obj[e][k].azi.toFixed(1),
|
||||
|
|
@ -162,6 +163,7 @@ function makeTable(str, obj)
|
|||
else if(gnssid==6)
|
||||
color="yellow";
|
||||
gnss_position.push([obj[e][k].azi, obj[e][k].elev, k.split("@")[0] , obj[e][k].db/4,4, color]);
|
||||
}
|
||||
});
|
||||
}
|
||||
else
|
||||
|
|
@ -215,7 +217,7 @@ function makeTable(str, obj)
|
|||
|
||||
return ret;
|
||||
})}).
|
||||
enter().append("td").text(function(d) {
|
||||
enter().append("td").html(function(d) {
|
||||
return d.value;
|
||||
|
||||
}).attr("align", d=> d.align).style("background-color", d=> d.color);
|
||||
|
|
|
|||
192
navdump.cc
192
navdump.cc
|
|
@ -26,7 +26,7 @@
|
|||
#include "sp3.hh"
|
||||
#include "ubx.hh"
|
||||
#include <unistd.h>
|
||||
|
||||
#include "sbas.hh"
|
||||
#include "version.hh"
|
||||
#include "gpscnav.hh"
|
||||
#include "rinex.hh"
|
||||
|
|
@ -37,7 +37,7 @@ using namespace std;
|
|||
|
||||
extern const char* g_gitHash;
|
||||
|
||||
Point g_ourpos;
|
||||
map<int, Point> g_srcpos;
|
||||
|
||||
|
||||
vector<SP3Entry> g_sp3s;
|
||||
|
|
@ -126,19 +126,27 @@ struct SVFilter
|
|||
pos = str.find(',', pos+1);
|
||||
if(pos != string::npos)
|
||||
satid.sigid = atoi(&str[pos+1]);
|
||||
// cout<<"Add to filter "<<satid.gnss<<", "<< satid.sv <<", "<<satid.sigid;
|
||||
d_filter.insert(satid);
|
||||
}
|
||||
bool check(int gnss, int sv, int sigid=-1)
|
||||
{
|
||||
// cout<<"Check filter "<<gnss<<", "<< sv <<", "<<sigid<<endl;
|
||||
if(d_filter.empty())
|
||||
return true;
|
||||
if(d_filter.count({gnss,0,-1})) // gnss match
|
||||
if(d_filter.count({gnss,0,-1})) { // gnss match
|
||||
// cout<<"gnss match"<<endl;
|
||||
return true;
|
||||
if(d_filter.count({gnss,sv,-1})) // gnss, sv match
|
||||
}
|
||||
if(d_filter.count({gnss,sv,-1})) { // gnss, sv match
|
||||
// cout<<"gnss, sv match"<<endl;
|
||||
return true;
|
||||
if(d_filter.count({gnss,sv,sigid})) // gnss, sv match, sigid
|
||||
}
|
||||
if(d_filter.count({gnss,sv,sigid})) { // gnss, sv match, sigid
|
||||
// cout<<"gnss, sv, sigid, match"<<endl;
|
||||
return true;
|
||||
|
||||
}
|
||||
// cout<<"Returning false"<<endl;
|
||||
return false;
|
||||
}
|
||||
set<SatID> d_filter;
|
||||
|
|
@ -179,7 +187,7 @@ void emitFixState(int src, double iTow, FixStat& fs, int n)
|
|||
|
||||
Point sat;
|
||||
getCoordinates(iTow, s.second.ephemeris, &sat);
|
||||
if(getElevationDeg(sat, g_ourpos) < 20)
|
||||
if(getElevationDeg(sat, g_srcpos[src]) < 20)
|
||||
continue;
|
||||
/*
|
||||
Point sat;
|
||||
|
|
@ -187,10 +195,10 @@ void emitFixState(int src, double iTow, FixStat& fs, int n)
|
|||
(void)trend;
|
||||
|
||||
getCoordinates(iTow + toffset/1000000000.0 + dt, s.second.ephemeris, &sat);
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
double range = Vector(g_srcpos[nmm.sourceid()], sat).length();
|
||||
|
||||
getCoordinates(iTow + range/299792458.0 + toffset/1000000000.0 + dt, s.second.ephemeris, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
range = Vector(g_srcpos[nmm.sourceid()], sat).length();
|
||||
*/
|
||||
double range = s.second.ephrange;
|
||||
if(s.second.bestrange1 != -1) {
|
||||
|
|
@ -214,13 +222,14 @@ void emitFixState(int src, double iTow, FixStat& fs, int n)
|
|||
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<<""<<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_srcpos[src]) << " 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
|
||||
{
|
||||
|
|
@ -235,13 +244,13 @@ try
|
|||
tles.parseFile("glo-ops.txt");
|
||||
tles.parseFile("gps-ops.txt");
|
||||
tles.parseFile("beidou.txt");
|
||||
/*
|
||||
readSP3s("all.sp3");
|
||||
|
||||
// readSP3s("all.sp3");
|
||||
if(!g_sp3s.empty()) {
|
||||
// sort(g_sp3s.begin(), g_sp3s.end(), [](const auto& a, const auto&b) { return a.t < b.t; });
|
||||
cout<<"Have "<<g_sp3s.size()<<" sp3 entries"<<endl; //, from "<<humanTime(g_sp3s.begin()->t) <<" to "<< humanTime(g_sp3s.rbegin()->t)<<endl;
|
||||
}
|
||||
*/
|
||||
|
||||
vector<string> svpairs;
|
||||
vector<int> stations;
|
||||
bool doReceptionData{false};
|
||||
|
|
@ -277,6 +286,7 @@ try
|
|||
ofstream iodstream("iodstream.csv");
|
||||
iodstream << "timestamp gnssid sv iodnav t0e age" << endl;
|
||||
|
||||
|
||||
ofstream csv("delta.csv");
|
||||
csv <<"timestamp gnssid sv tow tle_distance alma_distance utc_dist x y z vx vy vz rad inclination e iod"<<endl;
|
||||
|
||||
|
|
@ -286,12 +296,7 @@ try
|
|||
|
||||
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";
|
||||
|
||||
// RINEXNavWriter rnw("test.rnx");
|
||||
|
||||
for(;;) {
|
||||
char bert[4];
|
||||
int res = readn2(0, bert, 4);
|
||||
|
|
@ -494,7 +499,7 @@ try
|
|||
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);
|
||||
cout<<" ele " << getElevationDeg(satpos, g_srcpos[nmm.sourceid()]) << " azi " << getAzimuthDeg(satpos, g_srcpos[nmm.sourceid()]);
|
||||
}
|
||||
}
|
||||
else if(wtype == 8 && gm.tow - gmwtypes[{sv,7}].tow < 5 && gmwtypes[{sv,7}].alma1.svid && gm.iodalmanac == gmwtypes[{sv,7}].iodalmanac) {
|
||||
|
|
@ -526,13 +531,16 @@ try
|
|||
auto cond = getCondensedGPSMessage(std::basic_string<uint8_t>((uint8_t*)nmm.gpsi().contents().c_str(), nmm.gpsi().contents().size()));
|
||||
struct GPSState gs;
|
||||
static map<int, GPSState> eph;
|
||||
|
||||
static map<int, GPSAlmanac> almas;
|
||||
uint8_t page;
|
||||
static int gpswn;
|
||||
int frame=parseGPSMessage(cond, gs, &page);
|
||||
int frame=gs.parseGPSMessage(cond, &page);
|
||||
cout<<"GPS "<<sv<<"@"<<nmm.gpsi().sigid()<<": "<<gs.tow<<" frame "<<frame<<" ";
|
||||
static map<int, GPSState> oldgs1s;
|
||||
static map<int, GPSState> oldgs2s;
|
||||
if(frame == 1) {
|
||||
static map<int, GPSState> oldgs1s;
|
||||
|
||||
gpswn = gs.wn;
|
||||
cout << "gpshealth = "<<(int)gs.gpshealth<<", wn "<<gs.wn << " t0c "<<gs.t0c << " af0 " << gs.af0 << " af1 " << gs.af1 <<" af2 " << gs.af2;
|
||||
if(auto iter = oldgs1s.find(sv); iter != oldgs1s.end() && iter->second.t0c != gs.t0c) {
|
||||
|
|
@ -543,21 +551,23 @@ try
|
|||
oldgs1s[sv] = gs;
|
||||
}
|
||||
else if(frame == 2) {
|
||||
parseGPSMessage(cond, eph[sv], &page);
|
||||
cout << "t0e = "<<gs.iods.begin()->second.t0e << " " <<ephAge(gs.tow, gs.iods.begin()->second.t0e) << " iod "<<gs.gpsiod;
|
||||
eph[sv].parseGPSMessage(cond, &page);
|
||||
// gs in frame 2 contains t0e, so legit
|
||||
cout << "t0e = "<<gs.getT0e() << " " <<ephAge(gs.tow, gs.getT0e()) << " iod "<<gs.gpsiod;
|
||||
oldgs2s[sv] = gs;
|
||||
}
|
||||
else if(frame == 3) {
|
||||
parseGPSMessage(cond, eph[sv], &page);
|
||||
eph[sv].parseGPSMessage(cond, &page);
|
||||
cout <<"iod "<<gs.gpsiod;
|
||||
if(eph[sv].isComplete(gs.gpsiod)) {
|
||||
if(eph[sv].gpsiod == oldgs2s[sv].gpsiod) {
|
||||
Point sat;
|
||||
getCoordinates(gs.tow, eph[sv].iods[gs.gpsiod], &sat);
|
||||
getCoordinates(gs.tow, eph[sv], &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);
|
||||
cout<<" ele " << getElevationDeg(sat, g_srcpos[nmm.sourceid()]) << " azi " << getAzimuthDeg(sat, g_srcpos[nmm.sourceid()]);
|
||||
|
||||
if(almas.count(sv)) {
|
||||
Point almapoint;
|
||||
|
|
@ -565,9 +575,41 @@ try
|
|||
cout<<" alma-dist " << Vector(sat, almapoint).length();
|
||||
|
||||
Vector speed;
|
||||
getSpeed(gs.tow, eph[sv].iods[gs.gpsiod], &speed);
|
||||
getSpeed(gs.tow, eph[sv], &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;
|
||||
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].getI0()<<" " << eph[sv].getE() << " " <<gs.gpsiod<<endl;
|
||||
}
|
||||
|
||||
int start = utcFromGPS(gpswn, (int)gs.tow);
|
||||
cout<<"sp3 start: "<<start<<" wn " << gpswn<<" tow " << gs.tow << endl;
|
||||
|
||||
SP3Entry e{0, sv, start};
|
||||
auto bestSP3 = lower_bound(g_sp3s.begin(), g_sp3s.end(), e, sp3Order);
|
||||
if(bestSP3 != g_sp3s.end() && bestSP3->gnss == e.gnss && bestSP3->sv == sv) {
|
||||
static set<pair<int,int>> haveSeen;
|
||||
if(!haveSeen.count({sv, bestSP3->t})) {
|
||||
haveSeen.insert({sv, bestSP3->t});
|
||||
Point newPoint;
|
||||
double E=getCoordinates(gs.tow + (bestSP3->t - start), eph[sv], &newPoint, false);
|
||||
Point sp3Point(bestSP3->x, bestSP3->y, bestSP3->z);
|
||||
Vector dist(newPoint, sp3Point);
|
||||
|
||||
Vector nspeed;
|
||||
getSpeed(gs.tow + (bestSP3->t - start), eph[sv], &nspeed);
|
||||
Vector speed = nspeed;
|
||||
nspeed.norm();
|
||||
double along = nspeed.inner(dist);
|
||||
|
||||
cout<<"\nsp3 "<<(bestSP3->t - start)<<" G"<<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 << " " << getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first<< " " << dist.length()<< " ";
|
||||
cout << (bestSP3->clockBias - getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first);
|
||||
cout << " " << gs.af0 <<" " << gs.af1;
|
||||
cout << endl;
|
||||
|
||||
sp3csv <<std::fixed<< bestSP3->t << " 0 "<< sv <<" " << ephAge(gs.tow+(bestSP3->t - start), eph[sv].getT0e()) <<" "<<bestSP3->x<<" " << bestSP3->y<<" " <<bestSP3->z <<" " << newPoint.x<<" " <<newPoint.y <<" " <<newPoint.z << " " <<bestSP3->clockBias <<" ";
|
||||
sp3csv << getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first<<" " << dist.length() <<" " << along <<" ";
|
||||
sp3csv << (bestSP3->clockBias - getGPSAtomicOffset(gs.tow + (bestSP3->t-start), oldgs1s[sv]).first) << " " << E << " " << speed.length()<<endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -738,7 +780,7 @@ try
|
|||
else if(strno == 3)
|
||||
cout<<" l_n " << (int)gm.l_n << " z " <<gm.getZ()/1000.0;
|
||||
else if(strno == 4) {
|
||||
cout<<", taun "<<gm.taun <<" NT "<<gm.NT <<" FT " << (int) gm.FT <<" En " << (int)gm.En;
|
||||
cout<<", taun "<<gm.taun <<" NT "<<gm.NT <<" FT " << (int) gm.FT <<" En " << (int)gm.En <<" M " << (int)gm.M ;
|
||||
if(gm.x && gm.y && gm.z) {
|
||||
auto longlat = getLongLat(gm.getX(), gm.getY(), gm.getZ());
|
||||
cout<<" long "<< 180* longlat.first/M_PI <<" lat " << 180*longlat.second/M_PI<<" rad "<<gm.getRadius();
|
||||
|
|
@ -750,7 +792,7 @@ try
|
|||
}
|
||||
}
|
||||
else if(strno == 5)
|
||||
cout<<", n4 "<< (int)gm.n4 << " l_n " << gm.l_n;
|
||||
cout<<", n4 "<< (int)gm.n4 << " l_n " << gm.l_n <<" tauc "<< gm.tauc << " taugps "<<gm.taugps;
|
||||
else if(strno == 6 || strno ==8 || strno == 10 || strno ==12 ||strno ==14) {
|
||||
cout<<" nA "<< gm.nA <<" CnA " << gm.CnA <<" LambdaNaDeg "<< gm.getLambdaNaDeg() << " e " <<gm.getE() << " i0 "<< 180.0*gm.getI0()/M_PI;
|
||||
}
|
||||
|
|
@ -760,7 +802,8 @@ try
|
|||
cout<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
g_ourpos = Point(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
g_srcpos[nmm.sourceid()] = Point(nmm.op().x(), nmm.op().y(), nmm.op().z());
|
||||
|
||||
if(!doObserverPosition)
|
||||
continue;
|
||||
etstamp();
|
||||
|
|
@ -771,8 +814,8 @@ try
|
|||
<<" lat "<< 180*std::get<0>(latlonh)/M_PI
|
||||
<<" elev "<< std::get<2>(latlonh) << " acc "<<nmm.op().acc()<<" m "<<endl;
|
||||
|
||||
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";
|
||||
//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) {
|
||||
|
|
@ -818,16 +861,16 @@ try
|
|||
static int n;
|
||||
double E=getCoordinates(nmm.rfd().rcvtow(), eph, &sat);
|
||||
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
double range = Vector(g_srcpos[nmm.sourceid()], sat).length();
|
||||
|
||||
double origrange = range;
|
||||
E=getCoordinates(nmm.rfd().rcvtow() - range/299792458.0, eph, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
range = Vector(g_srcpos[nmm.sourceid()], 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();
|
||||
range = Vector(g_srcpos[nmm.sourceid()], sat).length();
|
||||
cout << " d "<< 10000.0*(origrange-range);
|
||||
origrange=range;
|
||||
*/
|
||||
|
|
@ -841,10 +884,10 @@ try
|
|||
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
|
||||
range = Vector(g_srcpos[nmm.sourceid()], 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;
|
||||
double rotcor = omegaE * (sat.x*g_srcpos[nmm.sourceid()].y - sat.y * g_srcpos[nmm.sourceid()].x) / 299792458.0;
|
||||
cout<<" rot-shift "<<rotcor;
|
||||
range += rotcor;
|
||||
|
||||
|
|
@ -874,7 +917,7 @@ try
|
|||
}
|
||||
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);
|
||||
auto dop = doDoppler(nmm.rfd().rcvtow(), g_srcpos[nmm.sourceid()], eph, 1575420000);
|
||||
satstat.radvel = dop.radvel;
|
||||
if(nmm.rfd().sigid()==1) {
|
||||
satstat.bestrange1 = bestrange;
|
||||
|
|
@ -910,6 +953,63 @@ try
|
|||
nmm.ujs().agccnt()<<" flags "<<nmm.ujs().flags()<<" jamind "<<
|
||||
nmm.ujs().jamind()<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::SBASMessageType) {
|
||||
if(!svfilter.check(1, nmm.sbm().gnsssv(), 0))
|
||||
continue;
|
||||
|
||||
etstamp();
|
||||
basic_string<uint8_t> sbas((uint8_t*)nmm.sbm().contents().c_str(), nmm.sbm().contents().size());
|
||||
cout<<" PRN "<<nmm.sbm().gnsssv()<<" SBAS message type ";
|
||||
|
||||
// Preamble sequence:
|
||||
// 0x53, 0x9a, 0xc6
|
||||
// 83, 154, 198
|
||||
|
||||
// preamble(8), msgtype(6), payload212-95(18)
|
||||
// 5 * payload194-3(32)
|
||||
// payload2-1 parity(24) pad(6)
|
||||
|
||||
|
||||
// cout<< makeHexDump(string((char*)sbas.c_str(), sbas.size())) << endl;
|
||||
int type = getbitu(&sbas[0], 8, 6);
|
||||
cout <<type<<" ";
|
||||
static map<int, SBASState> sbstate;
|
||||
|
||||
if(type == 0) {
|
||||
sbstate[nmm.sbm().gnsssv()].parse0(sbas, nmm.localutcseconds());
|
||||
}
|
||||
else if(type == 1) {
|
||||
sbstate[nmm.sbm().gnsssv()].parse1(sbas, nmm.localutcseconds());
|
||||
}
|
||||
else if(type == 6) {
|
||||
auto integ = sbstate[nmm.sbm().gnsssv()].parse6(sbas, nmm.localutcseconds());
|
||||
cout<<"integrity updated: ";
|
||||
for(const auto& i : integ) {
|
||||
cout<<makeSatPartialName(i.id)<<" corr "<<i.correction <<" udrei "<< i.udrei <<" ";
|
||||
}
|
||||
}
|
||||
else if(type ==7) {
|
||||
sbstate[nmm.sbm().gnsssv()].parse7(sbas, nmm.localutcseconds());
|
||||
cout<<" latency " <<sbstate[nmm.sbm().gnsssv()].d_latency;
|
||||
}
|
||||
else if(type == 24) {
|
||||
auto ret=sbstate[nmm.sbm().gnsssv()].parse24(sbas, nmm.localutcseconds());
|
||||
cout<< " fast";
|
||||
for(const auto& i : ret.first)
|
||||
cout<< " "<<makeSatPartialName(i.id)<<" corr "<< i.correction <<" udrei "<<i.udrei;
|
||||
for(const auto& i : ret.second)
|
||||
cout<< " "<<makeSatPartialName(i.id)<<" dx "<< i.dx <<" dy "<<i.dy<<" dz "<<i.dz<<" dai " <<i.dai;
|
||||
|
||||
}
|
||||
else if(type == 25) {
|
||||
auto ret = sbstate[nmm.sbm().gnsssv()].parse25(sbas, nmm.localutcseconds());
|
||||
for(const auto& i : ret)
|
||||
cout<< " "<<makeSatPartialName(i.id)<<" dx "<< i.dx <<" dy "<<i.dy<<" dz "<<i.dz<<" dai " <<i.dai;
|
||||
|
||||
}
|
||||
|
||||
cout<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::DebuggingType) {
|
||||
|
||||
auto res = parseTrkMeas(basic_string<uint8_t>((const uint8_t*)nmm.dm().payload().c_str(), nmm.dm().payload().size()));
|
||||
|
|
@ -932,13 +1032,13 @@ try
|
|||
const auto& eph = galEphemeris[a.sv];
|
||||
Point sat;
|
||||
getCoordinates(rtow, eph, &sat);
|
||||
elevA=getElevationDeg(sat, g_ourpos);
|
||||
elevA=getElevationDeg(sat, g_srcpos[nmm.sourceid()]);
|
||||
}
|
||||
if(galEphemeris.count(b.sv)) {
|
||||
const auto& eph = galEphemeris[b.sv];
|
||||
Point sat;
|
||||
getCoordinates(rtow, eph, &sat);
|
||||
elevB=getElevationDeg(sat, g_ourpos);
|
||||
elevB=getElevationDeg(sat, g_srcpos[nmm.sourceid()]);
|
||||
}
|
||||
return elevB < elevA;
|
||||
|
||||
|
|
@ -960,14 +1060,14 @@ try
|
|||
Point sat;
|
||||
|
||||
double E=getCoordinates(rtow - clockoffms/1000.0, eph, &sat);
|
||||
double range = Vector(g_ourpos, sat).length();
|
||||
double range = Vector(g_srcpos[nmm.sourceid()], sat).length();
|
||||
getCoordinates(rtow - clockoffms/1000.0 - range/299792458.0, eph, &sat);
|
||||
range = Vector(g_ourpos, sat).length();
|
||||
range = Vector(g_srcpos[nmm.sourceid()], 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;
|
||||
double rotcor = omegaE * (sat.x*g_srcpos[nmm.sourceid()].y - sat.y * g_srcpos[nmm.sourceid()].x) / 299792458.0;
|
||||
range += rotcor;
|
||||
|
||||
double bgdcor = 299792458.0 *ldexp(eph.BGDE1E5b,-32);
|
||||
|
|
@ -996,7 +1096,7 @@ try
|
|||
|
||||
cout<<" rotcor "<< rotcor;
|
||||
cout<<" relcor "<<relcor;
|
||||
cout<<" elev " << getElevationDeg(sat, g_ourpos);
|
||||
cout<<" elev " << getElevationDeg(sat, g_srcpos[nmm.sourceid()]);
|
||||
cout<<" bgd-m " << bgdcor;
|
||||
cout<<" clockoff-ms " << clockoffms << endl;
|
||||
|
||||
|
|
|
|||
|
|
@ -326,8 +326,8 @@ std::string sbasName(int prn)
|
|||
sbas ="GAGAN";
|
||||
}
|
||||
else
|
||||
sbas ="SBAS";
|
||||
sbas ="SBAS?";
|
||||
|
||||
sbas+="? " + std::to_string(prn);
|
||||
sbas+=" " + std::to_string(prn);
|
||||
return sbas;
|
||||
}
|
||||
|
|
|
|||
1188
navparse.cc
1188
navparse.cc
File diff suppressed because it is too large
Load Diff
136
navparse.hh
136
navparse.hh
|
|
@ -6,6 +6,9 @@
|
|||
#include "glonass.hh"
|
||||
#include <map>
|
||||
#include "tle.hh"
|
||||
#include "sbas.hh"
|
||||
#include "ephemeris.hh"
|
||||
|
||||
using namespace std; // XXX
|
||||
|
||||
struct SVPerRecv
|
||||
|
|
@ -18,128 +21,53 @@ struct SVPerRecv
|
|||
int qi{-1}; // quality indicator, -1 = unknown
|
||||
time_t t; // last seen
|
||||
};
|
||||
|
||||
struct SVIOD
|
||||
{
|
||||
std::bitset<32> words;
|
||||
int gnssid;
|
||||
uint32_t t0e;
|
||||
uint32_t e, sqrtA;
|
||||
int32_t m0, omega0, i0, omega, idot, omegadot, deltan;
|
||||
|
||||
int16_t cuc{0}, cus{0}, crc{0}, crs{0}, cic{0}, cis{0};
|
||||
// 60 seconds
|
||||
uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS
|
||||
|
||||
// 2^-34 2^-46
|
||||
int32_t af0{0} , af1{0};
|
||||
// 2^-59
|
||||
int8_t af2{0};
|
||||
|
||||
uint8_t sisa;
|
||||
|
||||
uint32_t wn{0}, tow{0};
|
||||
bool complete() const
|
||||
{
|
||||
if(gnssid==2)
|
||||
return words[1] && words[2] && words[3] && words[4];
|
||||
else
|
||||
return words[2] && words[3];
|
||||
}
|
||||
void addGalileoWord(std::basic_string_view<uint8_t> page);
|
||||
|
||||
double getMu() const
|
||||
{
|
||||
if(gnssid == 2) return 3.986004418 * pow(10.0, 14.0);
|
||||
if(gnssid == 0) return 3.986005 * pow(10.0, 14.0);
|
||||
throw std::runtime_error("getMu() called for unsupported gnssid "+to_string(gnssid));
|
||||
} // m^3/s^2
|
||||
// same for galileo & gps
|
||||
double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
|
||||
|
||||
uint32_t getT0e() const { return t0e; }
|
||||
uint32_t getT0c() const { return 60*t0c; }
|
||||
double getSqrtA() const { return ldexp(sqrtA, -19); }
|
||||
double getE() const { return ldexp(e, -33); }
|
||||
double getCuc() const { return ldexp(cuc, -29); } // radians
|
||||
double getCus() const { return ldexp(cus, -29); } // radians
|
||||
double getCrc() const { return ldexp(crc, -5); } // meters
|
||||
double getCrs() const { return ldexp(crs, -5); } // meters
|
||||
double getM0() const { return ldexp(m0 * M_PI, -31); } // radians
|
||||
double getDeltan()const { return ldexp(deltan *M_PI, -43); } //radians/s
|
||||
double getI0() const { return ldexp(i0 * M_PI, -31); } // radians
|
||||
double getCic() const { return ldexp(cic, -29); } // radians
|
||||
double getCis() const { return ldexp(cis, -29); } // radians
|
||||
double getOmegadot() const { return ldexp(omegadot * M_PI, -43); } // radians/s
|
||||
double getOmega0() const { return ldexp(omega0 * M_PI, -31); } // radians
|
||||
double getIdot() const { return ldexp(idot * M_PI, -43); } // radians/s
|
||||
double getOmega() const { return ldexp(omega * M_PI, -31); } // radians
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
/* Most of thes fields are raw, EXCEPT:
|
||||
t0t = seconds, raw fields are 3600 seconds (galileo), 4096 seconds (GPS)
|
||||
*/
|
||||
|
||||
class InfluxPusher;
|
||||
struct SVStat
|
||||
{
|
||||
uint8_t e5bhs{0}, e1bhs{0};
|
||||
uint8_t gpshealth{0};
|
||||
uint16_t ai0{0};
|
||||
int16_t ai1{0}, ai2{0};
|
||||
bool sf1{0}, sf2{0}, sf3{0}, sf4{0}, sf5{0};
|
||||
int BGDE1E5a{0}, BGDE1E5b{0};
|
||||
bool e5bdvs{false}, e1bdvs{false};
|
||||
|
||||
uint16_t wn{0}; // we put the "unrolled" week number here!
|
||||
uint32_t tow{0}; // "last seen"
|
||||
//
|
||||
// 2^-30 2^-50 1 8-bit week
|
||||
int32_t a0{0}, a1{0}, t0t{0}, wn0t{0};
|
||||
int32_t a0g{0}, a1g{0}, t0g{0}, wn0g{0};
|
||||
int8_t dtLS{0}, dtLSF{0};
|
||||
uint16_t wnLSF{0};
|
||||
uint8_t dn; // leap second day number
|
||||
// 1 2^-31 2^-43 2^-55 16 second
|
||||
int ura, af0, af1, af2, t0c, gpsiod; // GPS parameters that should not be here XXX (or perhaps they should)
|
||||
int gnss;
|
||||
|
||||
GPSState gpsmsg; // continuously being updated
|
||||
GPSState gpsmsg2, gpsmsg3; // new ephemeris being assembled here
|
||||
GPSState ephgpsmsg, oldephgpsmsg; // always has a consistent ephemeris
|
||||
GPSAlmanac gpsalma;
|
||||
|
||||
// beidou:
|
||||
int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1};
|
||||
BeidouMessage beidouMessage, oldBeidouMessage;
|
||||
BeidouMessage lastBeidouMessage1, lastBeidouMessage2;
|
||||
|
||||
int wn() const; // gets from the 'live' message
|
||||
int tow() const; // same
|
||||
|
||||
TLERepo::Match tleMatch;
|
||||
double lastTLELookupX{0};
|
||||
|
||||
// live, ephemeris
|
||||
BeidouMessage beidoumsg, ephBeidoumsg, oldephBeidoumsg;
|
||||
// internal
|
||||
BeidouMessage lastBeidouMessage1, lastBeidouMessage2;
|
||||
|
||||
// new galileo
|
||||
GalileoMessage galmsg;
|
||||
// consistent, live
|
||||
GalileoMessage ephgalmsg, galmsg, oldephgalmsg;
|
||||
// internal
|
||||
map<int, GalileoMessage> galmsgTyped;
|
||||
|
||||
// Glonass
|
||||
GlonassMessage glonassMessage;
|
||||
GlonassMessage ephglomsg, glonassMessage, oldephglomsg;
|
||||
pair<uint32_t, GlonassMessage> glonassAlmaEven;
|
||||
|
||||
map<uint64_t, SVPerRecv> perrecv;
|
||||
|
||||
double latestDisco{-1}, latestDiscoAge{-1}, timeDisco{-1000};
|
||||
|
||||
map<int, SVIOD> iods;
|
||||
void addGalileoWord(std::basic_string_view<uint8_t> page);
|
||||
|
||||
bool completeIOD() const;
|
||||
uint16_t getIOD() const;
|
||||
SVIOD liveIOD() const;
|
||||
SVIOD& getEph(int i) { return iods[i]; } // XXXX gps adaptor
|
||||
|
||||
pair<int,SVIOD> prevIOD{-1, SVIOD()};
|
||||
void clearPrev()
|
||||
{
|
||||
prevIOD.first = -1;
|
||||
}
|
||||
void checkCompleteAndClean(int iod);
|
||||
map<int, SBASCombo> sbas;
|
||||
|
||||
const GPSLikeEphemeris& liveIOD() const;
|
||||
const GPSLikeEphemeris& prevIOD() const;
|
||||
|
||||
bool completeIOD() const;
|
||||
double getCoordinates(double tow, Point* p, bool quiet=true) const;
|
||||
double getOldEphCoordinates(double tow, Point* p, bool quiet=true) const;
|
||||
void getSpeed(double tow, Vector* v) const;
|
||||
DopplerData doDoppler(double tow, const Point& us, double freq) const;
|
||||
void reportNewEphemeris(const SatID& id, InfluxPusher& idb);
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
7
sbas.hh
7
sbas.hh
|
|
@ -4,7 +4,7 @@
|
|||
#include <map>
|
||||
#include "navmon.hh"
|
||||
#include <vector>
|
||||
|
||||
#include "minivec.hh"
|
||||
|
||||
// 0 do not use
|
||||
// 1 PRN mask
|
||||
|
|
@ -17,6 +17,11 @@
|
|||
// 26 Ionospheric delay corrections
|
||||
// 27 SBAS service message
|
||||
|
||||
// GSA HQ Prague
|
||||
const Point c_egnosCenter{3970085, 1021937, 4869792};
|
||||
|
||||
// Somewhere in Minnesota, Dakota, Canada border
|
||||
const Point c_waasCenter{-510062, -4166466, 4786089};
|
||||
|
||||
struct SBASState
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue