lots
bert hubert
parent
d074b06f5f
commit
dbd807b268
33
beidou.hh
33
beidou.hh
|
|
@ -4,6 +4,7 @@
|
|||
#include "bits.hh"
|
||||
#include <math.h>
|
||||
#include <stdexcept>
|
||||
#include "ephemeris.hh"
|
||||
|
||||
std::basic_string<uint8_t> getCondensedBeidouMessage(std::basic_string_view<uint8_t> payload);
|
||||
int beidouBitconv(int their);
|
||||
|
|
@ -16,9 +17,6 @@ int beidouBitconv(int their);
|
|||
C05 (58.75E)
|
||||
*/
|
||||
|
||||
|
||||
|
||||
|
||||
struct BeidouMessage
|
||||
{
|
||||
uint8_t strtype;
|
||||
|
|
@ -70,11 +68,30 @@ struct BeidouMessage
|
|||
|
||||
uint8_t sath1, aodc, urai, aode;
|
||||
uint16_t t0c;
|
||||
int wn{-1}, a0, a1, a2;
|
||||
// 2^-33 2^-50 2^-66
|
||||
int wn{-1}, a0, a1, a2;
|
||||
// 2^17 2^30
|
||||
|
||||
uint32_t getT0c()
|
||||
{
|
||||
return 8 * t0c;
|
||||
}
|
||||
std::pair<double, double> getAtomicOffset(int Sow = -1)
|
||||
{
|
||||
if(Sow == -1)
|
||||
Sow = sow;
|
||||
int delta = ephAge(Sow, getT0c());
|
||||
double cur = a0 + ldexp(delta*a1, -17) + ldexp(delta*delta*a2, -30);
|
||||
double trend = ldexp(a1, -17) + ldexp(2*delta*a2, -30);
|
||||
|
||||
// now in units of 2^-33 seconds, which are ~0.116 nanoseconds each
|
||||
double factor = ldexp(1000000000, -33);
|
||||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
void parse1(std::basic_string_view<uint8_t> cond)
|
||||
{
|
||||
sath1 = bbitu(43,1 );
|
||||
sath1 = bbitu(43,1);
|
||||
aodc = bbitu(31+13, 5);
|
||||
urai = bbitu(31+12+1+5, 4);
|
||||
wn = bbitu(61, 13);
|
||||
|
|
@ -86,7 +103,7 @@ struct BeidouMessage
|
|||
}
|
||||
|
||||
int t0eMSB{-1};
|
||||
uint32_t sqrtA, e;
|
||||
uint32_t sqrtA{0}, e;
|
||||
int32_t deltan;
|
||||
int32_t cuc, cus, crc, crs;
|
||||
int32_t m0;
|
||||
|
|
@ -113,10 +130,7 @@ struct BeidouMessage
|
|||
crc = bbits(199, 18);
|
||||
crs = bbits(225, 18);
|
||||
sqrtA = bbitu(251, 32);
|
||||
|
||||
t0eMSB = bbitu(291, 2);
|
||||
|
||||
|
||||
}
|
||||
|
||||
int t0eLSB{-1};
|
||||
|
|
@ -143,7 +157,6 @@ struct BeidouMessage
|
|||
idot = bbits(190, 14);
|
||||
Omega0 = bbits(212, 32);
|
||||
omega = bbits(252, 32);
|
||||
|
||||
}
|
||||
|
||||
// 4 is all almanac
|
||||
|
|
|
|||
|
|
@ -10,6 +10,7 @@
|
|||
| tow t0e | -> 7*86400 - tow + t0e < 3.5 days, ok
|
||||
*/
|
||||
|
||||
// positive age = t0e in the past
|
||||
int ephAge(int tow, int t0e)
|
||||
{
|
||||
unsigned int diff;
|
||||
|
|
@ -29,3 +30,4 @@ int ephAge(int tow, int t0e)
|
|||
return tow - t0e; // in the future, negative age
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
36
ephemeris.hh
36
ephemeris.hh
|
|
@ -148,3 +148,39 @@ void getCoordinates(int wn, double tow, const T& iod, Point* p, bool quiet=true)
|
|||
|
||||
}
|
||||
|
||||
struct DopplerData
|
||||
{
|
||||
double preddop;
|
||||
double radvel;
|
||||
Vector speed;
|
||||
Point sat;
|
||||
double ephage;
|
||||
time_t t;
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
DopplerData doDoppler(int wn, int 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);
|
||||
Point core;
|
||||
Vector us2sat(us, ret.sat);
|
||||
getSpeed(wn, tow, eph, &ret.speed);
|
||||
Vector core2us(core, us);
|
||||
Vector dx(us, ret.sat); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
||||
us2sat.norm();
|
||||
ret.radvel=us2sat.inner(ret.speed);
|
||||
double c=299792458;
|
||||
ret.preddop = -freq * ret.radvel/c;
|
||||
|
||||
// be careful with time here -
|
||||
ret.ephage = ephAge(tow, eph.getT0e());
|
||||
// cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "<<nmm.rfd().doppler()<<endl;
|
||||
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
33
galileo.hh
33
galileo.hh
|
|
@ -3,6 +3,8 @@
|
|||
#include <string>
|
||||
#include <vector>
|
||||
#include <functional>
|
||||
#include "ephemeris.hh"
|
||||
#include "bits.hh"
|
||||
|
||||
bool getTOWFromInav(std::basic_string_view<uint8_t> inav, uint32_t *satTOW, uint16_t *wn);
|
||||
|
||||
|
|
@ -80,12 +82,12 @@ struct GalileoMessage
|
|||
|
||||
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
|
||||
uint16_t t0c;
|
||||
|
||||
// 2^-34 2^-46
|
||||
int32_t af0{-1} , af1{-1};
|
||||
int32_t af0{0} , af1{0};
|
||||
// 2^-59
|
||||
int8_t af2{-1};
|
||||
int8_t af2{0};
|
||||
|
||||
uint8_t sisa;
|
||||
|
||||
|
|
@ -132,6 +134,23 @@ struct GalileoMessage
|
|||
sisa = getbitu(&page[0], 120, 8);
|
||||
}
|
||||
|
||||
int getT0c()
|
||||
{
|
||||
return t0c * 60;
|
||||
}
|
||||
|
||||
std::pair<double, double> getAtomicOffset(int tow)
|
||||
{
|
||||
int delta = ephAge(tow, getT0c());
|
||||
double cur = af0 + ldexp(delta*af1, -12) + ldexp(delta*delta*af2, -25);
|
||||
double trend = ldexp(af1, -12) + ldexp(2*delta*af2, -25);
|
||||
|
||||
// now in units of 2^-34 seconds, which are ~0.058 nanoseconds each
|
||||
double factor = ldexp(1000000000, -34);
|
||||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
|
||||
// can't get enough of that ephemeris
|
||||
void parse4(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
|
|
@ -139,10 +158,10 @@ struct GalileoMessage
|
|||
cic = getbits(&page[0], 22, 16);
|
||||
cis = getbits(&page[0], 38, 16);
|
||||
|
||||
t0c = getbitu(&page[0], 54, 14);
|
||||
af0 = getbits(&page[0], 68, 31);
|
||||
af1 = getbits(&page[0], 99, 21);
|
||||
af2 = getbits(&page[0], 120, 6);
|
||||
t0c = getbitu(&page[0], 54, 14); // 60
|
||||
af0 = getbits(&page[0], 68, 31); // 2^-34
|
||||
af1 = getbits(&page[0], 99, 21); // 2^-46
|
||||
af2 = getbits(&page[0], 120, 6); // 2^-59
|
||||
}
|
||||
|
||||
// ionospheric disturbance, health, group delay, time
|
||||
|
|
|
|||
24
glonass.hh
24
glonass.hh
|
|
@ -28,7 +28,8 @@ struct GlonassMessage
|
|||
else if(strtype == 5) {
|
||||
parse5(gstr);
|
||||
}
|
||||
|
||||
else if(strtype == 6 || strtype ==8 || strtype == 10 || strtype ==12 ||strtype ==14)
|
||||
parse6_8_10_12_14(gstr);
|
||||
return strtype;
|
||||
}
|
||||
|
||||
|
|
@ -92,9 +93,14 @@ struct GlonassMessage
|
|||
*/
|
||||
|
||||
uint16_t NT;
|
||||
uint8_t FT, En, deltaTaun, M;
|
||||
uint32_t taun;
|
||||
uint8_t FT{255}, En, deltaTaun, M;
|
||||
int32_t taun; // 2^-30
|
||||
bool P4;
|
||||
|
||||
double getTaunNS()
|
||||
{
|
||||
return 1000*ldexp(1000000*taun, -30);
|
||||
}
|
||||
|
||||
void parse4(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
|
|
@ -104,7 +110,7 @@ struct GlonassMessage
|
|||
taun = getbitsglonass(&gstr[0], 85-80, 22);
|
||||
En = getbitu(&gstr[0], 85-53, 5);
|
||||
P4 = getbitu(&gstr[0], 85-34, 1);
|
||||
// missing delta tau n
|
||||
deltaTaun = getbitsglonass(&gstr[0], 85 - 58, 4);
|
||||
}
|
||||
|
||||
uint8_t n4; // counting from 1996 ('n4=1'), this is the 4-year plan index we are currently in
|
||||
|
|
@ -113,9 +119,17 @@ struct GlonassMessage
|
|||
{
|
||||
n4=getbitu(&gstr[0], 85-36, 5);
|
||||
taugps = getbitsglonass(&gstr[0], 85-31, 22);
|
||||
|
||||
}
|
||||
|
||||
|
||||
int nA;
|
||||
bool CnA;
|
||||
|
||||
|
||||
void parse6_8_10_12_14(std::basic_string_view<uint8_t> gstr)
|
||||
{
|
||||
CnA = getbitu(&gstr[0], 85-80, 1);
|
||||
nA = getbitu(&gstr[0], 85-77, 5);
|
||||
}
|
||||
|
||||
};
|
||||
|
|
|
|||
28
gps.hh
28
gps.hh
|
|
@ -4,6 +4,7 @@
|
|||
#include <map>
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload);
|
||||
|
||||
struct GPSState
|
||||
|
|
@ -17,12 +18,12 @@ struct GPSState
|
|||
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
|
||||
// 16 seconds
|
||||
uint16_t t0c;
|
||||
|
||||
// 2^-34 2^-46
|
||||
// 2^-31 2^-43
|
||||
int32_t af0 , af1;
|
||||
// 2^-59
|
||||
// ???
|
||||
int8_t af2;
|
||||
uint32_t wn{0}, tow{0};
|
||||
};
|
||||
|
|
@ -51,6 +52,25 @@ struct GPSState
|
|||
void checkCompleteAndClean(int iod){}
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
int getT0c(const T& eph)
|
||||
{
|
||||
return eph.t0c * 16;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
std::pair<double, double> getAtomicOffset(int tow, const T& eph)
|
||||
{
|
||||
int delta = ephAge(tow, getT0c(eph));
|
||||
double cur = eph.af0 + ldexp(delta*eph.af1, -12) + ldexp(delta*delta*eph.af2, -24);
|
||||
double trend = ldexp(eph.af1, -12) + ldexp(2*delta*eph.af2, -24);
|
||||
|
||||
// now in units of 2^-31 seconds, which are 0.5 nanoseconds each
|
||||
double factor = ldexp(1000000000, -31);
|
||||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
|
||||
// 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)
|
||||
|
|
|
|||
|
|
@ -14,7 +14,7 @@ function maketable(str, arr)
|
|||
enter().
|
||||
append("tr");
|
||||
|
||||
var columns = ["sv", "iod", "aode", "eph-age-m", "latest-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];
|
||||
var columns = ["sv", "iod", "aodc/e", "eph-age-m", "latest-disco", "sisa", "delta_hz_corr", "health", "a0", "a1","a0g", "a1g", "sources", "db", "elev", "last-seen-s"];
|
||||
|
||||
// append the header row
|
||||
thead.append("tr")
|
||||
|
|
@ -50,8 +50,13 @@ function maketable(str, arr)
|
|||
// ret.value="";
|
||||
ret.value += " "+row.sv;
|
||||
}
|
||||
else
|
||||
if(column == "eph-age-m") {
|
||||
else if(column == "aodc/e") {
|
||||
if(row["aodc"] != null && row["aode"] != null)
|
||||
ret.value = row["aodc"]+"/"+row["aode"];
|
||||
else
|
||||
ret.value="";
|
||||
}
|
||||
else if(column == "eph-age-m") {
|
||||
if(row[column] != null) {
|
||||
var b = moment.duration(-row[column], 'm');
|
||||
ret.value = b.humanize(true);
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ void MiniCurl::setupURL(const std::string& str, const ComboAddress* rem, const C
|
|||
curl_easy_setopt(d_curl, CURLOPT_PROTOCOLS, CURLPROTO_HTTP | CURLPROTO_HTTPS);
|
||||
curl_easy_setopt(d_curl, CURLOPT_SSL_VERIFYPEER, false);
|
||||
curl_easy_setopt(d_curl, CURLOPT_SSL_VERIFYHOST, false);
|
||||
curl_easy_setopt(d_curl, CURLOPT_FAILONERROR, true);
|
||||
// curl_easy_setopt(d_curl, CURLOPT_FAILONERROR, true);
|
||||
curl_easy_setopt(d_curl, CURLOPT_URL, str.c_str());
|
||||
curl_easy_setopt(d_curl, CURLOPT_WRITEFUNCTION, write_callback);
|
||||
curl_easy_setopt(d_curl, CURLOPT_WRITEDATA, this);
|
||||
|
|
@ -148,7 +148,7 @@ std::string MiniCurl::postURL(const std::string& str, const std::string& postdat
|
|||
long http_code = 0;
|
||||
curl_easy_getinfo(d_curl, CURLINFO_RESPONSE_CODE, &http_code);
|
||||
|
||||
if(res != CURLE_OK) {
|
||||
if(res != CURLE_OK || http_code >= 300 ) {
|
||||
cerr<<"Detailed error: "<<d_data<<endl;
|
||||
cerr<<postdata<<endl;
|
||||
throw std::runtime_error("Unable to post URL ("+std::to_string(http_code)+"): "+string(curl_easy_strerror(res)));
|
||||
|
|
|
|||
56
navdump.cc
56
navdump.cc
|
|
@ -84,6 +84,11 @@ int main(int argc, char** argv)
|
|||
|
||||
NavMonMessage nmm;
|
||||
nmm.ParseFromString(string(buffer, len));
|
||||
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType)
|
||||
continue;
|
||||
|
||||
|
||||
cout<<humanTime(nmm.localutcseconds())<<" "<<nmm.localutcnanoseconds()<<" ";
|
||||
cout<<"src "<<nmm.sourceid()<< " ";
|
||||
if(nmm.type() == NavMonMessage::ReceptionDataType) {
|
||||
|
|
@ -92,15 +97,25 @@ int main(int argc, char** argv)
|
|||
else if(nmm.type() == NavMonMessage::GalileoInavType) {
|
||||
basic_string<uint8_t> inav((uint8_t*)nmm.gi().contents().c_str(), nmm.gi().contents().size());
|
||||
static map<int, GalileoMessage> gms;
|
||||
static map<int, GalileoMessage> oldgm4s;
|
||||
GalileoMessage& gm = gms[nmm.gi().gnsssv()];
|
||||
int wtype = gm.parse(inav);
|
||||
|
||||
cout << "gal inav for "<<nmm.gi().gnssid()<<","<<nmm.gi().gnsssv()<<" tow "<< nmm.gi().gnsstow()<<" wtype "<< wtype << endl;
|
||||
static uint32_t tow;
|
||||
if(wtype == 4) {
|
||||
// 2^-34 2^-46
|
||||
cout <<" af0 "<<gm.af0 <<" af1 "<<gm.af1 <<", scaled: "<<ldexp(1.0*gm.af0, 19-34)<<", "<<ldexp(1.0*gm.af1, 38-46)<<endl;
|
||||
cout <<" af0 "<<gm.af0 <<" af1 "<<gm.af1 <<", scaled: "<<ldexp(1.0*gm.af0, 19-34)<<", "<<ldexp(1.0*gm.af1, 38-46);
|
||||
if(tow && oldgm4s.count(nmm.gi().gnsssv()) && oldgm4s[nmm.gi().gnsssv()].iodnav != gm.iodnav) {
|
||||
auto& oldgm4 = oldgm4s[nmm.gi().gnsssv()];
|
||||
auto oldOffset = oldgm4.getAtomicOffset(tow);
|
||||
auto newOffset = gm.getAtomicOffset(tow);
|
||||
cout<<" Timejump: "<<oldOffset.first - newOffset.first<<" after "<<(gm.getT0c() - oldgm4.getT0c() )<<" seconds";
|
||||
}
|
||||
cout<<endl;
|
||||
oldgm4s[nmm.gi().gnsssv()] = gm;
|
||||
}
|
||||
static uint32_t tow;
|
||||
|
||||
if(wtype == 0 || wtype == 5 || wtype == 6)
|
||||
tow = gm.tow;
|
||||
|
||||
|
|
@ -131,7 +146,15 @@ int main(int argc, char** argv)
|
|||
int frame=parseGPSMessage(cond, gs, &page);
|
||||
cout<<"GPS "<<sv<<": "<<gs.tow<<" ";
|
||||
if(frame == 1) {
|
||||
cout << "gpshealth = "<<(int)gs.gpshealth<<", wn "<<gs.wn;
|
||||
static map<int, GPSState> oldgs1s;
|
||||
|
||||
cout << "gpshealth = "<<(int)gs.gpshealth<<", wn "<<gs.wn << " t0c "<<gs.t0c;
|
||||
if(auto iter = oldgs1s.find(sv); iter != oldgs1s.end() && iter->second.t0c != gs.t0c) {
|
||||
auto oldOffset = getAtomicOffset(gs.tow, iter->second);
|
||||
auto newOffset = getAtomicOffset(gs.tow, gs);
|
||||
cout<<" Timejump: "<<oldOffset.first - newOffset.first<<" after "<<(getT0c(gs) - getT0c(iter->second) )<<" seconds, old t0c "<<iter->second.t0c;
|
||||
}
|
||||
oldgs1s[sv] = gs;
|
||||
}
|
||||
else if(frame == 2) {
|
||||
cout << "t0e = "<<gs.iods.begin()->second.t0e << " " <<ephAge(gs.tow, gs.iods.begin()->second.t0e);
|
||||
|
|
@ -146,7 +169,16 @@ int main(int argc, char** argv)
|
|||
int fraid = bm.parse(cond, &pageno);
|
||||
cout<<"BeiDou "<<sv<<": "<<bm.sow<<", FraID "<<fraid;
|
||||
if(fraid == 1) {
|
||||
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<<endl;
|
||||
static map<int, BeidouMessage> msgs;
|
||||
if(msgs[sv].wn>= 0 && msgs[sv].t0c != bm.t0c) {
|
||||
auto oldOffset = msgs[sv].getAtomicOffset(bm.sow);
|
||||
auto newOffset = bm.getAtomicOffset(bm.sow);
|
||||
cout<<" Timejump: "<<oldOffset.first - newOffset.first<<" after "<<(bm.getT0c() - msgs[sv].getT0c() )<<" seconds";
|
||||
}
|
||||
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;
|
||||
}
|
||||
else if(fraid == 4 && 1<= pageno && pageno <= 24) {
|
||||
cout <<" pageno "<< (int) pageno<<" AmEpID "<< getbitu(&cond[0], beidouBitconv(291), 2);
|
||||
|
|
@ -184,9 +216,23 @@ int main(int argc, char** argv)
|
|||
cout<<"BeiDou "<<sv<<" D2: "<<bm.sow<<", FraID "<<fraid << endl;
|
||||
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::GlonassInavType) {
|
||||
GlonassMessage gm;
|
||||
int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
|
||||
cout<<"Glonass R"<<nmm.gloi().gnsssv()<<" @ "<<nmm.gloi().freq() <<" strno "<<strno;
|
||||
if(strno == 1)
|
||||
cout << ", hour "<<(int)gm.hour <<" minute " <<(int)gm.minute <<" seconds "<<(int)gm.seconds;
|
||||
if(strno == 2)
|
||||
cout<<" Tb "<<(int)gm.Tb <<" Bn "<<(int)gm.Bn;
|
||||
else if(strno == 4)
|
||||
cout<<", taun "<<gm.taun <<" NT "<<gm.NT <<" FT " << (int) gm.FT <<" En " << (int)gm.En;
|
||||
else if(strno == 6 || strno ==8 || strno == 10 || strno ==12 ||strno ==14)
|
||||
cout<<" nA "<< gm.nA <<" CnA "<<gm.CnA;
|
||||
cout<<endl;
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::ObserverPositionType) {
|
||||
cout<<"ECEF"<<endl;
|
||||
// XXX!! this has to deal with source id!
|
||||
}
|
||||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
cout<<"RFdata for "<<nmm.rfd().gnssid()<<","<<nmm.rfd().gnsssv()<<endl;
|
||||
|
|
|
|||
12
navmon.proto
12
navmon.proto
|
|
@ -8,7 +8,7 @@ message NavMonMessage {
|
|||
RFDataType = 4;
|
||||
GPSInavType = 5;
|
||||
BeidouInavTypeD1 = 6;
|
||||
GLONASSInavType = 7;
|
||||
GlonassInavType = 7;
|
||||
BeidouInavTypeD2 = 8;
|
||||
}
|
||||
|
||||
|
|
@ -53,6 +53,13 @@ message NavMonMessage {
|
|||
required bytes contents =5;
|
||||
}
|
||||
|
||||
message GlonassInav {
|
||||
required uint32 gnssID = 1;
|
||||
required uint32 gnssSV = 2;
|
||||
required uint32 freq = 3;
|
||||
required bytes contents = 4;
|
||||
}
|
||||
|
||||
|
||||
message ReceptionData {
|
||||
required uint32 gnssID =1;
|
||||
|
|
@ -91,5 +98,6 @@ message NavMonMessage {
|
|||
optional ObserverPosition op=8;
|
||||
optional GPSInav gpsi=9;
|
||||
optional BeidouInavD1 bid1=10;
|
||||
optional BeidouInavD2 bid2=11;
|
||||
optional BeidouInavD2 bid2=11;
|
||||
optional GlonassInav gloi=12;
|
||||
}
|
||||
|
|
|
|||
284
navparse.cc
284
navparse.cc
|
|
@ -21,7 +21,7 @@
|
|||
#include "gps.hh"
|
||||
#include "glonass.hh"
|
||||
#include "beidou.hh"
|
||||
|
||||
#include "galileo.hh"
|
||||
#include <optional>
|
||||
using namespace std;
|
||||
struct EofException{};
|
||||
|
|
@ -35,6 +35,29 @@ struct GNSSReceiver
|
|||
|
||||
|
||||
int g_dtLS{18};
|
||||
uint64_t utcFromGST(int wn, int tow)
|
||||
{
|
||||
return (935280000 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGST(int wn, double tow)
|
||||
{
|
||||
return (935280000.0 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGPS(int wn, double tow)
|
||||
{
|
||||
return (315964800 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
|
||||
string humanFt(uint8_t ft)
|
||||
{
|
||||
static const char* ret[]={"100 cm", "200 cm", "250 cm", "400 cm", "500 cm", "7 m", "10 m", "12 m", "14 m", "16 m", "32 m", "64 m", "128 m", "256 m", "512 m", "NONE"};
|
||||
if(ft < 16)
|
||||
return ret[ft];
|
||||
return "???";
|
||||
}
|
||||
|
||||
|
||||
string humanSisa(uint8_t sisa)
|
||||
|
|
@ -76,9 +99,9 @@ struct SVIOD
|
|||
uint16_t t0c; // clock epoch, stored UNSCALED, since it is not in the same place as GPS
|
||||
|
||||
// 2^-34 2^-46
|
||||
int32_t af0 , af1;
|
||||
int32_t af0{0} , af1{0};
|
||||
// 2^-59
|
||||
int8_t af2;
|
||||
int8_t af2{0};
|
||||
|
||||
uint8_t sisa;
|
||||
|
||||
|
|
@ -197,13 +220,19 @@ struct SVStat
|
|||
int ura, af0, af1, af2, t0c; // GPS parameters that should not be here XXX
|
||||
|
||||
// beidou:
|
||||
int t0eMSB{-1}, t0eLSB{-1}, aode{-1};
|
||||
int t0eMSB{-1}, t0eLSB{-1}, aode{-1}, aodc{-1};
|
||||
BeidouMessage beidouMessage, oldBeidouMessage;
|
||||
BeidouMessage lastBeidouMessage2;
|
||||
BeidouMessage lastBeidouMessage1, lastBeidouMessage2;
|
||||
|
||||
// new galileo
|
||||
GalileoMessage galmsg;
|
||||
|
||||
// Glonass
|
||||
GlonassMessage glonassMessage;
|
||||
|
||||
map<uint64_t, SVPerRecv> perrecv;
|
||||
pair<uint32_t, double> deltaHz;
|
||||
double latestDisco{-1};
|
||||
double latestDisco{-1}, timeDisco{-1000};
|
||||
|
||||
map<int, SVIOD> iods;
|
||||
void addGalileoWord(std::basic_string_view<uint8_t> page);
|
||||
|
|
@ -323,11 +352,12 @@ void SVStat::addGalileoWord(std::basic_string_view<uint8_t> page)
|
|||
}
|
||||
}
|
||||
|
||||
void getSpeed(int wn, double tow, const SVIOD& iod, Vector* v)
|
||||
template<typename T>
|
||||
void getSpeed(int wn, double tow, const T& eph, Vector* v)
|
||||
{
|
||||
Point a, b;
|
||||
getCoordinates(wn, tow-0.5, iod, &a);
|
||||
getCoordinates(wn, tow+0.5, iod, &b);
|
||||
getCoordinates(wn, tow-0.5, eph, &a);
|
||||
getCoordinates(wn, tow+0.5, eph, &b);
|
||||
*v = Vector(a, b);
|
||||
}
|
||||
|
||||
|
|
@ -351,7 +381,7 @@ int latestTow(int gnssid)
|
|||
if(s.first.first == gnssid)
|
||||
ages[7*s.second.wn*86400 + s.second.tow]= s.first;
|
||||
if(ages.empty())
|
||||
throw runtime_error("Asked for latest WN: we don't know it yet");
|
||||
throw runtime_error("Asked for latest TOW for "+to_string(gnssid)+": we don't know it yet");
|
||||
return g_svstats[ages.rbegin()->second].tow;
|
||||
}
|
||||
|
||||
|
|
@ -366,25 +396,12 @@ uint64_t nanoTime(int gnssid, int wn, int tow)
|
|||
offset = 935280000;
|
||||
if(gnssid == 3) // Beidou, 01-01-2006
|
||||
offset = 1136073600;
|
||||
if(gnssid == 6) // GLONASS
|
||||
throw std::runtime_error("GLONASS does not have WN/TOW");
|
||||
if(gnssid == 6) { // GLONASS
|
||||
offset = 820368000;
|
||||
return 1000000000ULL*(offset + wn * 7*86400 + tow); // no leap seconds in glonass
|
||||
}
|
||||
|
||||
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS); // Leap!!
|
||||
}
|
||||
|
||||
uint64_t utcFromGST(int wn, int tow)
|
||||
{
|
||||
return (935280000 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGST(int wn, double tow)
|
||||
{
|
||||
return (935280000.0 + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
double utcFromGPS(int wn, double tow)
|
||||
{
|
||||
return (315964800 + wn * 7*86400 + tow - g_dtLS);
|
||||
return 1000000000ULL*(offset + wn * 7*86400 + tow - g_dtLS);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -409,8 +426,7 @@ struct InfluxPusher
|
|||
{
|
||||
if(g_svstats[id].wn ==0 && g_svstats[id].tow == 0)
|
||||
return;
|
||||
if(id.first == 3)
|
||||
cout << g_svstats[id].wn <<", "<<g_svstats[id].tow<<" -> " <<nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow)<<endl;
|
||||
// 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) + " value="+to_string(value)+" "+
|
||||
to_string(nanoTime(id.first, g_svstats[id].wn, g_svstats[id].tow))+"\n";
|
||||
|
||||
|
|
@ -620,10 +636,26 @@ try
|
|||
|
||||
item["gnssid"] = s.first.first;
|
||||
item["svid"] = s.first.second;
|
||||
if(s.first.first == 3) {
|
||||
// perhaps check oldBeidouMessage for sow >=0 as 'completeIOD'?
|
||||
|
||||
|
||||
if(s.first.first == 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;
|
||||
|
||||
if(time(0) - s.second.deltaHz.first < 60) {
|
||||
item["delta_hz"] = s.second.deltaHz.second;
|
||||
if(hzCorrection)
|
||||
item["delta_hz_corr"] = s.second.deltaHz.second - (1561.098/1575.42)* (*hzCorrection);
|
||||
}
|
||||
|
||||
}
|
||||
else if(s.first.first == 6) { // glonass
|
||||
if(s.second.glonassMessage.FT < 16)
|
||||
item["sisa"] = humanFt(s.second.glonassMessage.FT);
|
||||
item["aode"] = s.second.aode;
|
||||
item["iod"] = s.second.glonassMessage.Tb;
|
||||
}
|
||||
if(s.second.completeIOD()) {
|
||||
item["iod"]=s.second.getIOD();
|
||||
|
|
@ -644,7 +676,7 @@ try
|
|||
Point core;
|
||||
|
||||
// this should actually use local time!
|
||||
getCoordinates(latestWN(2), latestTow(2), s.second.liveIOD(), &p);
|
||||
getCoordinates(0, s.second.tow, s.second.liveIOD(), &p);
|
||||
|
||||
Vector core2us(core, our);
|
||||
Vector dx(our, p); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
|
|
@ -672,8 +704,11 @@ try
|
|||
if(s.first.first == 3) { // beidou
|
||||
item["a0g"]=s.second.a0g;
|
||||
item["a1g"]=s.second.a1g;
|
||||
if(s.second.aode > 0)
|
||||
if(s.second.aode >= 0)
|
||||
item["aode"]=s.second.aode;
|
||||
if(s.second.aodc >= 0)
|
||||
item["aodc"]=s.second.aodc;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -699,26 +734,30 @@ try
|
|||
item["healthissue"] = 2;
|
||||
|
||||
}
|
||||
else if(s.first.first == 0 || s.first.first == 3) {// gps or beidou
|
||||
else if(s.first.first == 0 || s.first.first == 3 || s.first.first == 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) {
|
||||
nlohmann::json det = nlohmann::json::object();
|
||||
det["elev"] = pr.second.el;
|
||||
det["db"] = pr.second.db;
|
||||
det["last-seen-s"] = time(0) - pr.second.t;
|
||||
perrecv[to_string(pr.first)]=det;
|
||||
if(pr.second.el > 0 && pr.second.el <= 90) {
|
||||
nlohmann::json det = nlohmann::json::object();
|
||||
det["elev"] = pr.second.el;
|
||||
det["db"] = pr.second.db;
|
||||
det["last-seen-s"] = time(0) - pr.second.t;
|
||||
perrecv[to_string(pr.first)]=det;
|
||||
}
|
||||
}
|
||||
item["perrecv"]=perrecv;
|
||||
|
||||
// xxx this is silly, should use local time
|
||||
item["last-seen-s"] = s.second.tow ? (7*86400*(latestWN(s.first.first) - s.second.wn) + latestTow(s.first.first) - (int)s.second.tow) : -1;
|
||||
item["last-seen-s"] = time(0) - nanoTime(s.first.first, s.second.wn, s.second.tow)/1000000000;
|
||||
if(s.second.latestDisco >=0) {
|
||||
item["latest-disco"]= s.second.latestDisco;
|
||||
}
|
||||
if(s.second.timeDisco > -100 && s.second.timeDisco < 100) {
|
||||
item["time-disco"]= s.second.timeDisco;
|
||||
}
|
||||
|
||||
|
||||
item["wn"] = s.second.wn;
|
||||
|
|
@ -779,8 +818,10 @@ try
|
|||
int sv = nmm.gi().gnsssv();
|
||||
pair<int, int> id={2,sv};
|
||||
g_svstats[id].wn = nmm.gi().gnsswn();
|
||||
|
||||
unsigned int wtype = getbitu(&inav[0], 0, 6);
|
||||
auto& svstat = g_svstats[id];
|
||||
auto oldgm = svstat.galmsg;
|
||||
|
||||
unsigned int wtype = svstat.galmsg.parse(inav);
|
||||
if(1) {
|
||||
// cout<<sv <<"\t" << wtype << "\t" << nmm.gi().gnsstow() << "\t"<< nmm.sourceid() << endl;
|
||||
/* if(g_svstats[id].tow > nmm.gi().gnsstow()) {
|
||||
|
|
@ -818,6 +859,14 @@ try
|
|||
|
||||
double offset = ldexp(1000.0*(1.0*g_svstats[id].iods[iod].af0 + ldexp(age*g_svstats[id].iods[iod].af1, -12)), -34);
|
||||
idb.addValue(id, "atomic_offset_ns", 1000000.0*offset);
|
||||
|
||||
if(oldgm.af0 && oldgm.t0c != svstat.galmsg.t0c) {
|
||||
auto oldOffset = oldgm.getAtomicOffset(svstat.tow);
|
||||
auto newOffset = svstat.galmsg.getAtomicOffset(svstat.tow);
|
||||
svstat.timeDisco = oldOffset.first - newOffset.first;
|
||||
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
;
|
||||
|
|
@ -940,44 +989,43 @@ try
|
|||
else if(nmm.type() == NavMonMessage::RFDataType) {
|
||||
int sv = nmm.rfd().gnsssv();
|
||||
pair<int,int> id{nmm.rfd().gnssid(), nmm.rfd().gnsssv()};
|
||||
if(g_svstats[id].completeIOD()) {
|
||||
Point sat;
|
||||
Point us=g_ourpos;
|
||||
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_ourpos, g_svstats[id].oldBeidouMessage, 1561.098 * 1000000);
|
||||
|
||||
// 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(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_svstats[id].liveIOD(), &sat);
|
||||
Point core;
|
||||
Vector us2sat(us, sat);
|
||||
Vector speed;
|
||||
getSpeed(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_svstats[id].liveIOD(), &speed);
|
||||
// cout<<sv<<" radius: "<<Vector(core, sat).length()<<", distance: "<<us2sat.length()<<", orbital velocity: "<<speed.length()/1000.0<<" km/s, ";
|
||||
if(isnan(res.preddop)) {
|
||||
cerr<<"Problem with doppler calculation for C"<<id.second<<": "<<endl;
|
||||
Point p;
|
||||
getCoordinates(0, nmm.rfd().rcvtow(), g_svstats[id].oldBeidouMessage, &p, false);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
Vector core2us(core, us);
|
||||
Vector dx(us, sat); // = x-ourx, dy = y-oury, dz = z-ourz;
|
||||
// double elev = acos ( core2us.inner(dx) / (core2us.length() * dx.length()));
|
||||
//double deg = 180.0* (elev/M_PI);
|
||||
// cout <<"elev: "<<90 - deg<< " ("<<g_svstats[id].el<<")\n";
|
||||
|
||||
us2sat.norm();
|
||||
double radvel=us2sat.inner(speed);
|
||||
double c=299792458;
|
||||
double freq = 1575.42 * 1000000; // frequency
|
||||
double preddop = -freq*radvel/c;
|
||||
|
||||
// be careful with time here -
|
||||
double ephage = ephAge(nmm.rfd().rcvtow(), g_svstats[id].liveIOD().t0e);
|
||||
// cout<<"Radial velocity: "<< radvel<<", predicted doppler: "<< preddop << ", measured doppler: "<<nmm.rfd().doppler()<<endl;
|
||||
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
||||
dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << preddop << " " << Vector(us, sat).length() << " " <<radvel <<" " << nmm.rfd().locktimems()<<" " <<ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
||||
dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << res.preddop << " " << Vector(g_ourpos, res.sat).length() << " " <<res.radvel <<" " << nmm.rfd().locktimems()<<" " <<res.ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
||||
nmm.rfd().dostd() << " "<<nmm.rfd().rcvtow()<<endl;
|
||||
|
||||
if(t - g_svstats[id].deltaHz.first > 10) {
|
||||
|
||||
g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop};
|
||||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
||||
auto corr = getHzCorrection(t);
|
||||
if(corr) {
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - (1561.098/1575.42) * (*corr));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
else if(g_svstats[id].completeIOD()) {
|
||||
auto res = doDoppler(nmm.rfd().rcvwn(), nmm.rfd().rcvtow(), g_ourpos, g_svstats[id].liveIOD(), 1575.42 * 1000000);
|
||||
time_t t = utcFromGPS(nmm.rfd().rcvwn(), nmm.rfd().rcvtow());
|
||||
dopplercsv << std::fixed << t <<" " << nmm.rfd().gnssid() <<" " <<sv<<" "<<nmm.rfd().pseudorange()<<" "<< nmm.rfd().carrierphase() <<" " << nmm.rfd().doppler()<<" " << res.preddop << " " << Vector(g_ourpos, res.sat).length() << " " <<res.radvel <<" " << nmm.rfd().locktimems()<<" " <<res.ephage << " " << nmm.rfd().prstd() << " " << nmm.rfd().cpstd() <<" " <<
|
||||
nmm.rfd().dostd() << endl;
|
||||
|
||||
if(t - g_svstats[id].deltaHz.first > 10) {
|
||||
g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - preddop};
|
||||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - preddop);
|
||||
g_svstats[id].deltaHz = {t, nmm.rfd().doppler() - res.preddop};
|
||||
idb.addValue(id, "delta_hz", nmm.rfd().doppler() - res.preddop);
|
||||
auto corr = getHzCorrection(t);
|
||||
if(corr) {
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - preddop - *corr);
|
||||
idb.addValue(id, "delta_hz_cor", nmm.rfd().doppler() - res.preddop - *corr);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -991,6 +1039,7 @@ try
|
|||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
auto& svstat = g_svstats[id];
|
||||
auto oldsvstat = svstat;
|
||||
uint8_t page;
|
||||
int frame=parseGPSMessage(cond, svstat, &page);
|
||||
if(frame == 1) {
|
||||
|
|
@ -1005,6 +1054,13 @@ try
|
|||
|
||||
double offset = ldexp(1000.0*(1.0*g_svstats[id].af0 + ldexp(age*g_svstats[id].af1, -12)), -31);
|
||||
idb.addValue(id, "atomic_offset_ns", 1000000.0*offset);
|
||||
|
||||
if(oldsvstat.af0 && oldsvstat.t0c != svstat.t0c) {
|
||||
auto oldOffset = getAtomicOffset(svstat.tow, oldsvstat);
|
||||
auto newOffset = getAtomicOffset(svstat.tow, svstat);
|
||||
svstat.timeDisco = oldOffset.first - newOffset.first;
|
||||
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
||||
}
|
||||
}
|
||||
else if(frame==2) {
|
||||
idb.addValue(id, "gpshealth", g_svstats[id].gpshealth);
|
||||
|
|
@ -1025,19 +1081,19 @@ try
|
|||
if(g_svstats[id].wn < 512)
|
||||
g_svstats[id].wn += 2048;
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavType) {
|
||||
pair<int,int> id{nmm.bi().gnssid(), nmm.bi().gnsssv()};
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD1) {
|
||||
pair<int,int> id{nmm.bid1().gnssid(), nmm.bid1().gnsssv()};
|
||||
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
|
||||
auto& svstat = g_svstats[id];
|
||||
uint8_t pageno;
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bi().contents().c_str(), nmm.bi().contents().size()));
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid1().contents().c_str(), nmm.bid1().contents().size()));
|
||||
auto& bm = svstat.beidouMessage;
|
||||
|
||||
int fraid=bm.parse(cond, &pageno);
|
||||
svstat.tow = nmm.bi().gnsstow();
|
||||
svstat.wn = nmm.bi().gnsswn();
|
||||
svstat.tow = nmm.bid1().gnsstow();
|
||||
svstat.wn = nmm.bid1().gnsswn();
|
||||
if(fraid == 1) {
|
||||
svstat.ura = bm.urai;
|
||||
svstat.gpshealth = bm.sath1;
|
||||
|
|
@ -1045,6 +1101,20 @@ try
|
|||
svstat.af1 = bm.a1;
|
||||
svstat.af2 = bm.a2;
|
||||
svstat.aode = bm.aode;
|
||||
svstat.aodc = bm.aodc;
|
||||
idb.addValue(id, "atomic_offset_ns", 1000000.0*bm.getAtomicOffset().first);
|
||||
idb.addValue(id, "t0c", bm.getT0c());
|
||||
idb.addValue(id, "af0", bm.a0 * 2);
|
||||
idb.addValue(id, "af1", bm.a1 / 16);
|
||||
idb.addValue(id, "af2", bm.a2 / 128); // scaled to galileo units
|
||||
if(svstat.lastBeidouMessage1.wn >=0 && svstat.lastBeidouMessage1.t0c != bm.t0c) {
|
||||
auto oldOffset = svstat.lastBeidouMessage1.getAtomicOffset(bm.sow);
|
||||
auto newOffset = bm.getAtomicOffset(bm.sow);
|
||||
svstat.timeDisco = oldOffset.first - newOffset.first;
|
||||
idb.addValue(id, "clock_jump_ns", svstat.timeDisco);
|
||||
|
||||
}
|
||||
svstat.lastBeidouMessage1 = bm;
|
||||
}
|
||||
if(fraid == 2) {
|
||||
svstat.lastBeidouMessage2 = bm;
|
||||
|
|
@ -1060,9 +1130,16 @@ try
|
|||
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,
|
||||
newpoint.x, newpoint.y, newpoint.z, jump.length(), (double)bm.getT0e() - svstat.oldBeidouMessage.getT0e());
|
||||
svstat.latestDisco = jump.length();
|
||||
double hours = (bm.getT0e() - svstat.oldBeidouMessage.getT0e())/3600;
|
||||
if(hours < 4) {
|
||||
svstat.latestDisco = jump.length();
|
||||
idb.addValue(id, "eph-disco", jump.length());
|
||||
}
|
||||
else
|
||||
svstat.latestDisco = -1;
|
||||
}
|
||||
svstat.oldBeidouMessage = bm;
|
||||
if(bm.sqrtA) // only copy in if complete
|
||||
svstat.oldBeidouMessage = bm;
|
||||
}
|
||||
if(fraid==5 && pageno == 9) {
|
||||
svstat.a0g = bm.a0gps;
|
||||
|
|
@ -1077,7 +1154,52 @@ try
|
|||
|
||||
getCoordinates(svstat.wn, svstat.tow, bm, &sat);
|
||||
Vector l(core, sat);
|
||||
cout<<"C"<<id.second<< " "<<bm.sow<<" "<<(bm.sow % 30 )<<" FraID "<<fraid<<" "<<fmt::format("({0}, {1}, {2})", sat.x, sat.y, sat.z) <<", r: "<<l.length()<<" elev: "<<getElevation(sat)<<endl;
|
||||
// cout<<"C"<<id.second<< " "<<bm.sow<<" "<<(bm.sow % 30 )<<" FraID "<<fraid<<" "<<fmt::format("({0}, {1}, {2})", sat.x, sat.y, sat.z) <<", r: "<<l.length()<<" elev: "<<getElevation(sat)<<endl;
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::BeidouInavTypeD2) {
|
||||
auto cond = getCondensedBeidouMessage(std::basic_string<uint8_t>((uint8_t*)nmm.bid2().contents().c_str(), nmm.bid2().contents().size()));
|
||||
int fraid = getbitu(&cond[0], beidouBitconv(16), 3);
|
||||
int sow = getbitu(&cond[0], beidouBitconv(19), 20);
|
||||
int pnum = getbitu(&cond[0], beidouBitconv(43), 4);
|
||||
int pre = getbitu(&cond[0], beidouBitconv(1), 11);
|
||||
(void) pre;
|
||||
// cout<<"C"<< nmm.bid2().gnsssv() << " sent D2 message, pre "<<pre<<" sow "<<sow<<", FraID "<<fraid;
|
||||
// if(fraid == 1)
|
||||
// cout <<" pnum "<<pnum;
|
||||
// cout<<endl;
|
||||
}
|
||||
else if(nmm.type()== NavMonMessage::GlonassInavType) {
|
||||
pair<int,int> id{nmm.gloi().gnssid(), nmm.gloi().gnsssv()};
|
||||
auto& svstat = g_svstats[id];
|
||||
|
||||
auto& gm = svstat.glonassMessage;
|
||||
int strno = gm.parse(std::basic_string<uint8_t>((uint8_t*)nmm.gloi().contents().c_str(), nmm.gloi().contents().size()));
|
||||
g_svstats[id].perrecv[nmm.sourceid()].t = nmm.localutcseconds();
|
||||
if(strno == 1) {
|
||||
struct tm tm;
|
||||
memset(&tm, 0, sizeof(tm));
|
||||
tm.tm_year = 119;
|
||||
tm.tm_mon = 8 - 1;
|
||||
tm.tm_mday = 30;
|
||||
tm.tm_hour = gm.hour - 3;
|
||||
tm.tm_min = gm.minute;
|
||||
tm.tm_sec = gm.seconds;
|
||||
|
||||
time_t t = timegm(&tm);
|
||||
uint32_t glotime = t - 820368000; // this starts GLONASS time at 31st of december 1995, 00:00 UTC
|
||||
svstat.wn = glotime / (7*86400);
|
||||
svstat.tow = glotime % (7*86400);
|
||||
|
||||
}
|
||||
if(strno == 2) {
|
||||
svstat.gpshealth = gm.Bn;
|
||||
}
|
||||
if(strno == 4) {
|
||||
svstat.aode = gm.En * 24;
|
||||
idb.addValue(id, "glo_taun_ns", gm.getTaunNS());
|
||||
idb.addValue(id, "ft", gm.FT);
|
||||
}
|
||||
cout<<"GLONASS R"<<id.second<<" str "<<strno<<endl;
|
||||
}
|
||||
else {
|
||||
cout<<"Unknown type "<< (int)nmm.type()<<endl;
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ void recvSession(int s, ComboAddress client)
|
|||
break;
|
||||
}
|
||||
if(part != "bert") {
|
||||
cerr << "Wrong magic from "<<client.toStringWithPort()<<endl;
|
||||
cerr << "Wrong magic from "<<client.toStringWithPort()<<": "<<part<<endl;
|
||||
break;
|
||||
}
|
||||
string out=part;
|
||||
|
|
@ -82,6 +82,7 @@ void recvSession(int s, ComboAddress client)
|
|||
catch(std::exception& e) {
|
||||
cout<<"Error in receiving thread: "<<e.what()<<endl;
|
||||
}
|
||||
cout<<"Thread exiting"<<endl;
|
||||
}
|
||||
|
||||
void recvListener(Socket&& s, ComboAddress local)
|
||||
|
|
|
|||
|
|
@ -34,7 +34,7 @@ vector<string> getPathComponents(std::string_view root, time_t s, uint64_t sourc
|
|||
|
||||
ret.push_back(to_string(tm.tm_year+1900));
|
||||
ret.push_back(to_string(tm.tm_mon+1));
|
||||
ret.push_back(to_string(tm.tm_mday+1));
|
||||
ret.push_back(to_string(tm.tm_mday));
|
||||
ret.push_back(to_string(tm.tm_hour)+".pb");
|
||||
return ret;
|
||||
}
|
||||
|
|
|
|||
467
ubxdisplay.cc
467
ubxdisplay.cc
|
|
@ -1,467 +0,0 @@
|
|||
#include <stdio.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <arpa/inet.h>
|
||||
#include "fmt/format.h"
|
||||
#include "fmt/printf.h"
|
||||
#include <fstream>
|
||||
#include <map>
|
||||
#include <bitset>
|
||||
#include <curses.h>
|
||||
#include <vector>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
using namespace std;
|
||||
|
||||
|
||||
struct EofException{};
|
||||
|
||||
uint8_t getUint8()
|
||||
{
|
||||
int c;
|
||||
c = fgetc(stdin);
|
||||
if(c == -1)
|
||||
throw EofException();
|
||||
return (uint8_t) c;
|
||||
}
|
||||
|
||||
uint16_t getUint16()
|
||||
{
|
||||
uint16_t ret;
|
||||
int res = fread(&ret, 1, sizeof(ret), stdin);
|
||||
if(res != sizeof(ret))
|
||||
throw EofException();
|
||||
// ret = ntohs(ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* lovingly lifted from RTKLIB */
|
||||
unsigned int getbitu(const unsigned char *buff, int pos, int len)
|
||||
{
|
||||
unsigned int bits=0;
|
||||
int i;
|
||||
for (i=pos;i<pos+len;i++) bits=(bits<<1)+((buff[i/8]>>(7-i%8))&1u);
|
||||
return bits;
|
||||
}
|
||||
|
||||
extern int getbits(const unsigned char *buff, int pos, int len)
|
||||
{
|
||||
unsigned int bits=getbitu(buff,pos,len);
|
||||
if (len<=0||32<=len||!(bits&(1u<<(len-1)))) return (int)bits;
|
||||
return (int)(bits|(~0u<<len)); /* extend sign */
|
||||
}
|
||||
|
||||
|
||||
uint16_t calcUbxChecksum(uint8_t ubxClass, uint8_t ubxType, std::basic_string_view<uint8_t> str)
|
||||
{
|
||||
uint8_t CK_A = 0, CK_B = 0;
|
||||
|
||||
auto update = [&CK_A, &CK_B](uint8_t c) {
|
||||
CK_A = CK_A + c;
|
||||
CK_B = CK_B + CK_A;
|
||||
};
|
||||
update(ubxClass);
|
||||
update(ubxType);
|
||||
uint16_t len = str.size();
|
||||
update(((uint8_t*)&len)[0]);
|
||||
update(((uint8_t*)&len)[1]);
|
||||
for(unsigned int I=0; I < str.size(); I++) {
|
||||
update(str[I]);
|
||||
}
|
||||
return (CK_B << 8) + CK_A;
|
||||
}
|
||||
|
||||
struct SVIOD
|
||||
{
|
||||
std::bitset<32> words;
|
||||
uint32_t t0e;
|
||||
uint8_t sisa;
|
||||
bool complete() const
|
||||
{
|
||||
return words[1] && words[3];
|
||||
}
|
||||
void addWord(std::basic_string_view<uint8_t> page);
|
||||
};
|
||||
|
||||
void SVIOD::addWord(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
uint8_t wtype = getbitu(&page[0], 0, 6);
|
||||
words[wtype]=true;
|
||||
if(wtype == 1) {
|
||||
t0e = 60*getbitu(&page[0], 16, 14);
|
||||
}
|
||||
else if(wtype == 3) {
|
||||
sisa = getbitu(&page[0], 120, 8);
|
||||
}
|
||||
}
|
||||
|
||||
struct SVStat
|
||||
{
|
||||
uint8_t e5bhs{0}, e1bhs{0};
|
||||
bool e5bdvs{false}, e1bdvs{false};
|
||||
bool disturb1{false}, disturb2{false}, disturb3{false}, disturb4{false}, disturb5{false};
|
||||
uint16_t wn{0};
|
||||
uint32_t tow{0}; // "last seen"
|
||||
uint32_t a0{0}, a1{0};
|
||||
int el{0}, db{0};
|
||||
map<int, SVIOD> iods;
|
||||
void addWord(std::basic_string_view<uint8_t> page);
|
||||
bool completeIOD() const;
|
||||
uint16_t getIOD() const;
|
||||
SVIOD liveIOD() const;
|
||||
};
|
||||
|
||||
bool SVStat::completeIOD() const
|
||||
{
|
||||
for(const auto& iod : iods)
|
||||
if(iod.second.complete())
|
||||
return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t SVStat::getIOD() const
|
||||
{
|
||||
for(const auto& iod : iods)
|
||||
if(iod.second.complete())
|
||||
return iod.first;
|
||||
throw std::runtime_error("Asked for unknown IOD");
|
||||
}
|
||||
|
||||
SVIOD SVStat::liveIOD() const
|
||||
{
|
||||
if(auto iter = iods.find(getIOD()); iter != iods.end())
|
||||
return iter->second;
|
||||
throw std::runtime_error("Asked for unknown IOD");
|
||||
}
|
||||
|
||||
void SVStat::addWord(std::basic_string_view<uint8_t> page)
|
||||
{
|
||||
uint8_t wtype = getbitu(&page[0], 0, 6);
|
||||
if(wtype >=1 && wtype <= 4) { // ephemeris
|
||||
uint16_t iod = getbitu(&page[0], 6, 10);
|
||||
iods[iod].addWord(page);
|
||||
if(iods[iod].complete()) {
|
||||
SVIOD latest = iods[iod];
|
||||
iods.clear();
|
||||
iods[iod] = latest;
|
||||
}
|
||||
}
|
||||
else if(wtype==5) { // disturbance, health, time
|
||||
e5bhs = getbitu(&page[0], 67, 2);
|
||||
e1bhs = getbitu(&page[0], 69, 2);
|
||||
e5bdvs = getbitu(&page[0], 71, 1);
|
||||
e1bdvs = getbitu(&page[0], 72, 1);
|
||||
wn = getbitu(&page[0], 73, 12);
|
||||
tow = getbitu(&page[0], 85, 20);
|
||||
// cout<<"Setting tow to "<<tow<<endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
struct WinKeeper
|
||||
{
|
||||
WinKeeper();
|
||||
struct Window
|
||||
{
|
||||
WINDOW *header, *text;
|
||||
int sv;
|
||||
void setHeader(std::string_view line)
|
||||
{
|
||||
wclear(header);
|
||||
wmove(header, 0, 0);
|
||||
wattron(header, A_BOLD | A_UNDERLINE);
|
||||
wprintw(header, "%s", &line[0]);
|
||||
wattroff(header, A_BOLD | A_UNDERLINE);
|
||||
wrefresh(header);
|
||||
}
|
||||
void setStatus(std::string_view line)
|
||||
{
|
||||
wmove(header, 1, 0);
|
||||
wattron(header, A_BOLD);
|
||||
wprintw(header, "%s", &line[0]);
|
||||
wattroff(header, A_BOLD);
|
||||
wrefresh(header);
|
||||
}
|
||||
|
||||
void emitLine(std::string_view line)
|
||||
{
|
||||
wprintw(text, "%s\n", &line[0]);
|
||||
wrefresh(text);
|
||||
}
|
||||
};
|
||||
vector<Window> d_windows;
|
||||
static int d_h, d_w;
|
||||
void emitLine(int sv, std::string_view line);
|
||||
void setStatus(int sv, std::string_view line);
|
||||
};
|
||||
|
||||
int WinKeeper::d_h;
|
||||
int WinKeeper::d_w;
|
||||
|
||||
WinKeeper::WinKeeper()
|
||||
{
|
||||
initscr();
|
||||
getmaxyx(stdscr, d_h, d_w);
|
||||
|
||||
int winwidth = d_w / 7;
|
||||
for(int n=0; n < 8 ; ++n) {
|
||||
d_windows.push_back({
|
||||
newwin(3, winwidth, 0, n*(winwidth+2)),
|
||||
newwin(d_h-3, winwidth, 3, n*(winwidth+2)),
|
||||
0});
|
||||
scrollok(d_windows[n].text, 1);
|
||||
}
|
||||
};
|
||||
|
||||
void WinKeeper::emitLine(int sv, std::string_view line)
|
||||
{
|
||||
for(auto& w: d_windows) {
|
||||
if(w.sv == sv) {
|
||||
w.emitLine(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// nothing matched
|
||||
for(auto& w: d_windows) {
|
||||
if(!w.sv) {
|
||||
w.sv = sv;
|
||||
w.setHeader(std::to_string(sv));
|
||||
w.emitLine(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv));
|
||||
}
|
||||
|
||||
void WinKeeper::setStatus(int sv, std::string_view line)
|
||||
{
|
||||
for(auto& w: d_windows) {
|
||||
if(w.sv == sv) {
|
||||
w.setStatus(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
// nothing matched
|
||||
for(auto& w: d_windows) {
|
||||
if(!w.sv) {
|
||||
w.sv = sv;
|
||||
w.setHeader(std::to_string(sv));
|
||||
w.setStatus(line);
|
||||
return;
|
||||
}
|
||||
}
|
||||
throw std::runtime_error("Ran out of windows searching for sv "+std::to_string(sv));
|
||||
}
|
||||
|
||||
|
||||
std::map<int, SVStat> g_svstats;
|
||||
|
||||
int main()
|
||||
try
|
||||
{
|
||||
WinKeeper wk;
|
||||
|
||||
// unsigned int tow=0, wn=0;
|
||||
ofstream csv("iod.csv");
|
||||
ofstream csv2("toe.csv");
|
||||
csv<<"timestamp sv iod sisa"<<endl;
|
||||
csv2<<"timestamp sv tow toe"<<endl;
|
||||
int tow=0, wn=0;
|
||||
string line;
|
||||
for(;;) {
|
||||
auto c = getUint8();
|
||||
if(c != 0xb5) {
|
||||
// cout << (char)c;
|
||||
line.append(1,c);
|
||||
if(c=='\n') {
|
||||
if(line.rfind("$GAGSV", 0)==0) {
|
||||
vector<string> strs;
|
||||
boost::split(strs,line,boost::is_any_of(","));
|
||||
for(unsigned int n=4; n + 4 < strs.size(); n += 4) {
|
||||
int sv = atoi(strs[n].c_str());
|
||||
|
||||
g_svstats[sv].el = atoi(strs[n+1].c_str());
|
||||
g_svstats[sv].db = atoi(strs[n+3].c_str());
|
||||
|
||||
}
|
||||
}
|
||||
line.clear();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
c = getUint8();
|
||||
if(c != 0x62) {
|
||||
ungetc(c, stdin); // might be 0xb5
|
||||
continue;
|
||||
}
|
||||
|
||||
// if we are here, just had ubx header
|
||||
|
||||
uint8_t ubxClass = getUint8();
|
||||
uint8_t ubxType = getUint8();
|
||||
uint16_t msgLen = getUint16();
|
||||
|
||||
// cout <<"Had an ubx message of class "<<(int) ubxClass <<" and type "<< (int) ubxType << " of " << msgLen <<" bytes"<<endl;
|
||||
|
||||
std::basic_string<uint8_t> msg;
|
||||
msg.reserve(msgLen);
|
||||
for(int n=0; n < msgLen; ++n)
|
||||
msg.append(1, getUint8());
|
||||
|
||||
uint16_t ubxChecksum = getUint16();
|
||||
if(ubxChecksum != calcUbxChecksum(ubxClass, ubxType, msg)) {
|
||||
cout<<"Checksum: "<<ubxChecksum<< ", calculated: "<<
|
||||
calcUbxChecksum(ubxClass, ubxType, msg)<<"\n";
|
||||
}
|
||||
|
||||
if(ubxClass == 2 && ubxType == 89) { // SAR
|
||||
string hexstring;
|
||||
for(int n = 0; n < 15; ++n)
|
||||
hexstring+=fmt::format("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4));
|
||||
|
||||
int sv = (int)msg[2];
|
||||
wk.emitLine(sv, "SAR "+hexstring);
|
||||
// cout<<"SAR: sv = "<< (int)msg[2] <<" ";
|
||||
// for(int n=4; n < 12; ++n)
|
||||
// fmt::printf("%02x", (int)msg[n]);
|
||||
|
||||
// for(int n = 0; n < 15; ++n)
|
||||
// fmt::printf("%x", (int)getbitu(msg.c_str(), 36 + 4*n, 4));
|
||||
|
||||
// cout << " Type: "<< (int) msg[12] <<"\n";
|
||||
// cout<<"Parameter: (len = "<<msg.length()<<") ";
|
||||
// for(unsigned int n = 13; n < msg.length(); ++n)
|
||||
// fmt::printf("%02x ", (int)msg[n]);
|
||||
// cout<<"\n";
|
||||
}
|
||||
if(ubxClass == 2 && ubxType == 19) { //UBX-RXM-SFRBX
|
||||
// cout<<"SFRBX GNSSID "<< (int)msg[0]<<", SV "<< (int)msg[1];
|
||||
// cout<<" words "<< (int)msg[4]<<", version "<< (int)msg[6];
|
||||
// cout<<"\n";
|
||||
if(msg[0] != 2) // version field
|
||||
continue;
|
||||
// 7 is reserved
|
||||
|
||||
// cout << ((msg[8]&128) ? "Even " : "Odd ");
|
||||
// cout << ((msg[8]&64) ? "Alert " : "Nominal ");
|
||||
unsigned int wtype = (int)(msg[11] & (~(64+128)));
|
||||
unsigned int sv = (int)msg[1];
|
||||
// cout << "Word type "<< (int)(msg[11] & (~(64+128))) <<" SV " << (int)msg[1]<<"\n";
|
||||
// for(unsigned int n = 8; n < msg.size() ; ++n) {
|
||||
// fmt::printf("%02x ", msg[n]);
|
||||
// }
|
||||
// cout<<"\n";
|
||||
std::basic_string<uint8_t> payload;
|
||||
for(unsigned int i = 0 ; i < (msg.size() - 8) / 4; ++i)
|
||||
for(int j=1; j <= 4; ++j)
|
||||
payload.append(1, msg[8 + (i+1) * 4 -j]);
|
||||
|
||||
// for(auto& c : payload)
|
||||
// fmt::printf("%02x ", c);
|
||||
|
||||
// cout<<"\n";
|
||||
|
||||
std::basic_string<uint8_t> inav;
|
||||
unsigned int i,j;
|
||||
for (i=0,j=2; i<14; i++, j+=8)
|
||||
inav.append(1, (unsigned char)getbitu(payload.c_str() ,j,8));
|
||||
for (i=0,j=2; i< 2; i++, j+=8)
|
||||
inav.append(1, (unsigned char)getbitu(payload.c_str()+16,j,8));
|
||||
|
||||
// cout<<"inav for "<<wtype<<" for sv "<<sv<<": ";
|
||||
// for(auto& c : inav)
|
||||
// fmt::printf("%02x ", c);
|
||||
|
||||
g_svstats[sv].addWord(inav);
|
||||
if(wtype >=1 && wtype <= 4) { // ephemeris
|
||||
uint16_t iod = getbitu(&inav[0], 6, 10);
|
||||
if(wtype == 1 && tow) {
|
||||
int t0e = 60*getbitu(&inav[0], 16, 14);
|
||||
int age = (tow - t0e)/60;
|
||||
uint32_t e = getbitu(&inav[0], 6+10+14+32, 32);
|
||||
wk.emitLine(sv, "Eph" +std::to_string(wtype)+", e=" + to_string(e)+", age="+to_string(age)+"m" );
|
||||
}
|
||||
else if(wtype == 3) {
|
||||
unsigned int sisa = getbitu(&inav[0], 120, 8);
|
||||
wk.emitLine(sv, "Eph3, iod="+to_string(iod)+", sisa="+to_string(sisa));
|
||||
}
|
||||
else
|
||||
wk.emitLine(sv, "Eph" +std::to_string(wtype)+", iod=" + to_string(iod));
|
||||
}
|
||||
else if(!wtype) {
|
||||
wk.emitLine(sv, ".");
|
||||
if(getbitu(&inav[0], 6,2) == 2) {
|
||||
wn = getbitu(&inav[0], 96, 12);
|
||||
tow = getbitu(&inav[0], 108, 20);
|
||||
}
|
||||
}
|
||||
else if(wtype == 5) {
|
||||
string out="IonoBGDHealth ";
|
||||
for(int n=0; n <5; ++n)
|
||||
out.append(1, getbitu(&inav[0], 42+n, 1) ? '1' : '0');
|
||||
out+=" ";
|
||||
out += to_string(getbitu(&inav[0], 67,2))+to_string(getbitu(&inav[0], 69,2))+to_string(getbitu(&inav[0], 70,1))+to_string(getbitu(&inav[0], 71,1));
|
||||
wk.emitLine(sv, out);
|
||||
wk.setStatus(sv, "Hlth: "+std::to_string(getbitu(&inav[0], 67, 2)) +", el="+to_string(g_svstats[sv].el)+", db="+to_string(g_svstats[sv].db) );
|
||||
tow = getbitu(&inav[0], 85, 20);
|
||||
wn = getbitu(&inav[0], 73, 12);
|
||||
}
|
||||
else if(wtype == 6) {
|
||||
int a0 = getbits(&inav[0], 6, 32);
|
||||
int a1 = getbits(&inav[0], 38, 24);
|
||||
int t0t = getbitu(&inav[0], 70, 8);
|
||||
uint8_t wn0t = getbits(&inav[0], 78, 8);
|
||||
int dw = (uint8_t)wn - wn0t;
|
||||
if(tow && wn)
|
||||
wk.emitLine(sv, "GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1)+", age="+to_string(tow/3600-t0t)+"h, dw="+to_string(dw)
|
||||
+", wn0t="+to_string(wn0t)+", wn8="+to_string(wn&0xff));
|
||||
else
|
||||
wk.emitLine(sv, "GST-UTC6, a0="+to_string(a0)+", a1="+to_string(a1));
|
||||
|
||||
}
|
||||
else if(wtype >= 7 && wtype <= 9) {
|
||||
uint16_t ioda = getbitu(&inav[0], 6, 4);
|
||||
wk.emitLine(sv, "Alm"+to_string(wtype)+" IODa="+to_string(ioda));
|
||||
}
|
||||
else if(wtype == 10) {
|
||||
int a0g = getbits(&inav[0], 86, 16);
|
||||
int a1g = getbits(&inav[0], 102, 12);
|
||||
int t0g = getbitu(&inav[0], 114, 8);
|
||||
uint8_t wn0g = getbitu(&inav[0], 122, 6);
|
||||
int dw = (((uint8_t)wn)&(1+2+4+8+16+32)) - wn0g;
|
||||
|
||||
if(tow && wn) {
|
||||
time_t t = 935280000 + wn * 7*86400 + tow;
|
||||
struct tm tm;
|
||||
gmtime_r(&t, &tm);
|
||||
|
||||
int age = tow - t0g * 3600;
|
||||
// a0g = 2^-32 s, a1 = 2^-50 s/s
|
||||
|
||||
// int shift = a0g * (1U<<16) + a1g * age; // in 2^-51 seconds units
|
||||
|
||||
wk.emitLine(sv, "GST-GPS, a0g="+to_string(a0g)+", a1g="+to_string(a1g)+", t0g="+to_string(t0g)+", age="+to_string(tow/3600-t0g)+"h, dw="+to_string(dw)
|
||||
+", wn0g="+to_string(wn0g)+", wn6="+to_string(wn&(1+2+4+8+16+32)));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
else
|
||||
wk.emitLine(sv, "Word "+std::to_string(wtype));
|
||||
|
||||
// time_t t = 935280000 + wn * 7*86400 + tow;
|
||||
/*
|
||||
for(const auto& ent : g_svstats) {
|
||||
// 12 iod t0e
|
||||
fmt::printf("%2d\t", ent.first);
|
||||
if(ent.second.completeIOD()) {
|
||||
cout << ent.second.getIOD() << "\t" << ( ent.second.tow - ent.second.liveIOD().t0e ) << "\t" << (unsigned int)ent.second.liveIOD().sisa;
|
||||
}
|
||||
cout<<"\n";
|
||||
}
|
||||
cout<<"\n";
|
||||
*/
|
||||
}
|
||||
}
|
||||
}
|
||||
catch(EofException& e)
|
||||
{}
|
||||
23
ubxtool.cc
23
ubxtool.cc
|
|
@ -432,7 +432,7 @@ int main(int argc, char** argv)
|
|||
// 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, 0x04, 0x08, 0, doGlonass, 0x00, 0x01, 0x00
|
||||
0x06, 0x06, 0x08, 0, doGlonass, 0x00, 0x01, 0x00
|
||||
|
||||
});
|
||||
|
||||
|
|
@ -779,14 +779,23 @@ int main(int argc, char** argv)
|
|||
continue;
|
||||
}
|
||||
else if(id.first==6) {
|
||||
cerr<<"SFRBX from GLONASS "<<id.second<<" @ frequency "<<(int)payload[3]<<", msg of "<<(int)payload[4]<< " words"<<endl;
|
||||
// cerr<<"SFRBX from GLONASS "<<id.second<<" @ frequency "<<(int)payload[3]<<", msg of "<<(int)payload[4]<< " words"<<endl;
|
||||
auto gstr = getGlonassFromSFRBXMsg(payload);
|
||||
static map<int, GlonassMessage> gms;
|
||||
GlonassMessage& gm = gms[id.second];
|
||||
int strno = gm.parse(gstr);
|
||||
cerr<<"R"<<id.second<<" "<<strno<<" ("<<gm.x<<", "<<gm.y<<", "<<gm.z<<") "<<sqrt(ldexp(gm.x, -11)*ldexp(gm.x, -11) + ldexp(gm.y, -11)*ldexp(gm.y, -11) + ldexp(gm.z, -11)*ldexp(gm.z, -11)) <<" -> ("<<gm.dx<<", "<<gm.dy<<", "<<gm.dz<<")"<<endl;
|
||||
if(strno == 4)
|
||||
cerr<<" R"<<id.second<<" "<< fmt::sprintf("%d %02d:%02d:%02d", gm.NT, (int)gm.hour,(int)gm.minute, (int)gm.seconds)<<" : P4="<<gm.P4<<" -> " << (int)gm.Tb<<", P1: "<<(int)gm.P1<<endl;
|
||||
if(id.second != 255) {
|
||||
NavMonMessage nmm;
|
||||
nmm.set_localutcseconds(g_gstutc.tv_sec);
|
||||
nmm.set_localutcnanoseconds(g_gstutc.tv_nsec);
|
||||
nmm.set_sourceid(g_srcid);
|
||||
nmm.set_type(NavMonMessage::GlonassInavType);
|
||||
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_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;
|
||||
|
|
@ -797,7 +806,9 @@ int main(int argc, char** argv)
|
|||
payload[0], payload[1], wtype, lasttv[id].tv_sec, lasttv[id].tv_usec, tv[id].tv_sec, tv[id].tv_usec, tv[id].tv_usec - lasttv[id].tv_usec);
|
||||
}
|
||||
#endif
|
||||
lasttv[id]=tv[id];
|
||||
lasttv[id]=tv[id];
|
||||
|
||||
|
||||
}
|
||||
catch(CRCMismatch& cm) {
|
||||
cerr<<"Had CRC mismatch!"<<endl;
|
||||
|
|
|
|||
Loading…
Reference in New Issue