teach protobuf about GPS CNAV
bert hubert
parent
2973b66d19
commit
923c54f724
|
|
@ -0,0 +1,20 @@
|
|||
/*
|
||||
cerr<<"Preamble "<<getbitu(&cnav[0], 0, 8)<<" sv "<<
|
||||
getbitu(&cnav[0], 8, 6) << " type "<< type <<
|
||||
" tow " << tow;
|
||||
|
||||
if(type == 10) {
|
||||
wn = getbitu(&cnav[0], 38, 13);
|
||||
cerr<<" t0p " << 300*getbitu(&cnav[0], 54,11)<< " t0e "<< 300*getbitu(&cnav[0], 70, 11);
|
||||
}
|
||||
else if(type == 11) {
|
||||
cerr<<" t0e "<< 300* getbitu(&cnav[0], 38, 11);
|
||||
|
||||
|
||||
}
|
||||
else if(type == 32) {
|
||||
cerr<< " delta-ut1 " << 1000.0*ldexp(getbits(&cnav[0], 215, 31), -24)<< "ms rate " << 1000.0*ldexp(getbits(&cnav[0], 246, 19), -25) << " te0p " <<
|
||||
16*getbits(&cnav[0], 127, 16);
|
||||
|
||||
}
|
||||
*/
|
||||
|
|
@ -0,0 +1,227 @@
|
|||
#pragma once
|
||||
#include <bitset>
|
||||
#include <string>
|
||||
#include <map>
|
||||
#include "bits.hh"
|
||||
#include <iostream>
|
||||
#include <math.h>
|
||||
|
||||
struct GPSCNavAlmanac
|
||||
{
|
||||
int dataid{-1};
|
||||
int sv;
|
||||
uint32_t t0a{0};
|
||||
uint32_t e{0}, sqrtA{0};
|
||||
int32_t M0, Omega0, deltai, omega, omegadot;
|
||||
int health;
|
||||
int af0, af1;
|
||||
|
||||
double getMu() const
|
||||
{
|
||||
return 3.986005 * pow(10.0, 14.0);
|
||||
} // m^3/s^2
|
||||
// same for galileo & gps
|
||||
double getOmegaE() const { return 7.2921151467 * pow(10.0, -5.0);} // rad/s
|
||||
|
||||
|
||||
double getE() const
|
||||
{
|
||||
return ldexp(e, -21);
|
||||
}
|
||||
double getT0e() const
|
||||
{
|
||||
return ldexp(t0a, 12);
|
||||
}
|
||||
|
||||
double getI0() const
|
||||
{
|
||||
return M_PI*0.3 + ldexp(M_PI*deltai, -19);
|
||||
}
|
||||
|
||||
double getOmegadot() const
|
||||
{
|
||||
return ldexp(M_PI * omegadot, -38);
|
||||
}
|
||||
|
||||
double getSqrtA() const
|
||||
{
|
||||
return ldexp(sqrtA, -11);
|
||||
}
|
||||
|
||||
double getOmega0() const
|
||||
{
|
||||
return ldexp(M_PI * Omega0, -23);
|
||||
}
|
||||
double getOmega() const
|
||||
{
|
||||
return ldexp(M_PI * omega, -23);
|
||||
}
|
||||
|
||||
double getM0() const
|
||||
{
|
||||
return ldexp(M_PI * M0, -23);
|
||||
}
|
||||
double getIdot() const { return 0; } // radians/s
|
||||
double getCic() const { return 0; } // radians
|
||||
double getCis() const { return 0; } // radians
|
||||
double getCuc() const { return 0; } // radians
|
||||
double getCus() const { return 0; } // radians
|
||||
double getCrc() const { return 0; } // meters
|
||||
double getCrs() const { return 0; } // meters
|
||||
double getDeltan()const { return 0; } //radians/s
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
struct GPSCNavState
|
||||
{
|
||||
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;
|
||||
|
||||
// 2^-31 2^-43
|
||||
int32_t af0 , af1;
|
||||
// ???
|
||||
int8_t af2;
|
||||
uint32_t wn{0}, tow{0};
|
||||
|
||||
double getMu() const
|
||||
{
|
||||
return 3.986005 * pow(10.0, 14.0);
|
||||
} // 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; }
|
||||
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
|
||||
|
||||
|
||||
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};
|
||||
//
|
||||
// 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
|
||||
|
||||
|
||||
int gpsiod{-1};
|
||||
|
||||
int teop;
|
||||
int deltaUT1;
|
||||
int deltaUT1dot;
|
||||
|
||||
// milliseconds
|
||||
std::pair<double, double> getUT1OffsetMS(int tow)
|
||||
{
|
||||
int delta = ephAge(tow, 16* teop);
|
||||
std::cout<<" tow "<<tow<<" teop "<< 16*teop <<" delta "<<delta<<" " << 1000.0*ldexp(deltaUT1, -24)<<"ms ";
|
||||
double cur = ldexp(deltaUT1 + ldexp(delta * deltaUT1dot / 86400.0, -1), -24);
|
||||
double trend = ldexp(deltaUT1dot, -25);
|
||||
return {1000.0* cur, 1000.0*trend};
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
template<typename T>
|
||||
std::pair<double, double> getGPSCNavAtomicOffset(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};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
std::pair<double, double> getGPSCNavUTCOffset(int tow, int wn, const T& eph)
|
||||
{
|
||||
// 2^-30 2^-50 3600
|
||||
// a0 a1 t0t
|
||||
|
||||
int dw = (int)(uint8_t)wn - (int)(uint8_t) eph.wn0t;
|
||||
int delta = dw*7*86400 + tow - eph.t0t; // this is pre-scaled for GPS..
|
||||
|
||||
double cur = eph.a0 + ldexp(1.0*delta*eph.a1, -20);
|
||||
double trend = ldexp(eph.a1, -20);
|
||||
|
||||
// now in units of 2^-30 seconds, which are ~1.1 nanoseconds each
|
||||
|
||||
double factor = ldexp(1000000000, -30);
|
||||
return {factor * cur, factor * trend};
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
template<typename T>
|
||||
int parseGPSCNavMessage(std::basic_string_view<uint8_t> msg, T& out)
|
||||
{
|
||||
using namespace std;
|
||||
int type = getbitu(&msg[0], 14, 6);
|
||||
out.tow = 6 * getbitu(&msg[0], 20, 17) - 12;
|
||||
|
||||
if(type == 10) {
|
||||
out.wn = getbitu(&msg[0], 38, 13);
|
||||
// out.ura = getbitu(&msg[0], 65, 5);
|
||||
out.t0e = getbitu(&msg[0], 70, 11); // XXX scale?
|
||||
|
||||
// int32_t deltaSqrtA = getbits(&msg[0], 81, 26);
|
||||
// int32_t SqrtADot = getbits(&msg[0], 107, 25);
|
||||
out.deltan = getbitu(&msg[0], 132, 17); // XXX scale??
|
||||
// int deltandot = getbitu(&msg[0], 149, 23); // XXX scale??
|
||||
// int64_t M0n = getbitu(&msg[0], 172, 33); // XXX scale??
|
||||
|
||||
out.e = getbitu(&msg[0], 205, 33); // XXX scale??
|
||||
out.omega = getbitu(&msg[0], 238, 33); // XXX scale?
|
||||
}
|
||||
else if(type == 11) {
|
||||
out.t0e = getbitu(&msg[0], 38, 11); // XXX scale?
|
||||
out.omega0 = getbitu(&msg[0], 49, 33); // XXX scale?
|
||||
out.i0 = getbitu(&msg[0], 82, 33); // XXX scale?
|
||||
// int64_t deltaOmegaDot = getbitu(&msg[0], 115, 17); // XXX scale?
|
||||
out.idot = getbitu(&msg[0], 132, 15); // XXX scale?
|
||||
out.cis = getbits(&msg[0], 147, 16);// XXX scale?
|
||||
out.cic = getbits(&msg[0], 163, 16);// XXX scale?
|
||||
out.crs = getbits(&msg[0], 179, 24);// XXX scale?
|
||||
out.crc = getbits(&msg[0], 203, 24);// XXX scale?
|
||||
out.cus = getbits(&msg[0], 227, 21);// XXX scale?
|
||||
out.cuc = getbits(&msg[0], 248, 21); // XXX scale?
|
||||
}
|
||||
else if(type == 32) {
|
||||
out.deltaUT1 = getbits(&msg[0], 215, 31);
|
||||
out.deltaUT1dot = getbits(&msg[0], 246, 19);
|
||||
out.teop = getbitu(&msg[0], 127, 16);
|
||||
}
|
||||
return type;
|
||||
}
|
||||
13
navmon.proto
13
navmon.proto
|
|
@ -15,6 +15,7 @@ message NavMonMessage {
|
|||
ObserverDetailsType = 11;
|
||||
UbloxJammingStatsType = 12;
|
||||
SBASMessageType = 13;
|
||||
GPSCnavType = 14;
|
||||
}
|
||||
|
||||
required uint64 sourceID = 1;
|
||||
|
|
@ -160,6 +161,17 @@ message NavMonMessage {
|
|||
}
|
||||
|
||||
|
||||
message GPSCnav {
|
||||
required uint32 gnssWN =1;
|
||||
required uint32 gnssTOW =2; // INTEGERS!
|
||||
|
||||
required uint32 gnssID =3;
|
||||
required uint32 gnssSV =4;
|
||||
required bytes contents =5;
|
||||
required uint32 sigid = 6;
|
||||
}
|
||||
|
||||
|
||||
optional GalileoInav gi=5;
|
||||
optional ReceptionData rd=6;
|
||||
optional RFData rfd=7;
|
||||
|
|
@ -173,4 +185,5 @@ message NavMonMessage {
|
|||
optional ObserverDetails od = 15;
|
||||
optional UbloxJammingStats ujs = 16;
|
||||
optional SBASMessage sbm = 17;
|
||||
optional GPSCnav gpsc=18;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue