329 lines
9.8 KiB
C++
329 lines
9.8 KiB
C++
#include "sbas.hh"
|
|
#include <iostream>
|
|
using namespace std;
|
|
#include "bits.hh"
|
|
#include <math.h>
|
|
|
|
void SBASState::parse0(const basic_string<uint8_t>& sbas, time_t now)
|
|
{
|
|
d_lastDNU = now;
|
|
d_lastSeen = now;
|
|
}
|
|
|
|
|
|
void SBASState::parse1(const basic_string<uint8_t>& sbas, time_t now)
|
|
{
|
|
d_lastSeen = now;
|
|
int slot=1;
|
|
d_slot2prn.clear();
|
|
for(int prn = 0; prn < 210; ++prn) {
|
|
if(getbitu(&sbas[0], 14+ prn, 1)) {
|
|
d_slot2prn[slot]=prn+1;
|
|
// cout<<slot<<"=G"<<prn+1<<" ";
|
|
slot++;
|
|
}
|
|
}
|
|
}
|
|
|
|
vector<SBASState::FastCorrection> SBASState::parse2_5(const basic_string<uint8_t>&sbas, time_t now)
|
|
{
|
|
d_lastSeen = now;
|
|
int type = getbitu(&sbas[0], 8, 6);
|
|
vector<SBASState::FastCorrection> ret;
|
|
// IODFi, IODP, 13*12 bits fast correction, 13*4 bits UDREI
|
|
for(int pos = 0; pos < 13; ++pos) {
|
|
int slot = 1+13*(type-2)+pos;
|
|
|
|
if(d_slot2prn.count(slot)) {
|
|
FastCorrection fc;
|
|
fc.id = getSBASSatID(slot);
|
|
fc.udrei = getbitu(&sbas[0], 14 + 4 + 12*13 + 4*pos, 4);
|
|
fc.correction = getbits(&sbas[0], 14 + 4 + 12*pos, 12)*0.125;
|
|
fc.lastUpdate=now;
|
|
ret.push_back(fc);
|
|
d_fast[fc.id] = fc;
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
vector<SBASState::FastCorrection> SBASState::parse6(const basic_string<uint8_t>&sbas, time_t now)
|
|
{
|
|
d_lastSeen = now;
|
|
vector<SBASState::FastCorrection> ret;
|
|
|
|
for(int slot = 0; slot < 51; ++slot) {
|
|
SatID sid = getSBASSatID(slot + 1);
|
|
if(sid.gnss == 255)
|
|
continue;
|
|
if(!d_fast.count(sid))
|
|
continue;
|
|
|
|
FastCorrection& fc = d_fast[sid];
|
|
fc.id = sid;
|
|
fc.udrei = getbitu(&sbas[0], 14 + 8 + 4* slot, 4);
|
|
fc.lastUpdate = now;
|
|
ret.push_back(fc);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void SBASState::parse7(const basic_string<uint8_t>&sbas, time_t now)
|
|
{
|
|
d_lastSeen = now;
|
|
d_latency = getbitu(&sbas[0], 14+4, 4);
|
|
}
|
|
|
|
int SBASState::getSBASNumber(int slot) const
|
|
{
|
|
auto iter = d_slot2prn.find(slot);
|
|
if(iter != d_slot2prn.end()) {
|
|
int prn = iter->second;
|
|
if(prn < 37)
|
|
return prn;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
SatID SBASState::getSBASSatID(int slot) const
|
|
{
|
|
SatID ret;
|
|
auto iter = d_slot2prn.find(slot);
|
|
if(iter != d_slot2prn.end()) {
|
|
int prn = iter->second;
|
|
if(prn < 37) {
|
|
ret.gnss = 0;
|
|
ret.sv = prn;
|
|
ret.sigid = 0;
|
|
return ret;
|
|
}
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
vector<SBASState::LongTermCorrection> SBASState::parse25(const basic_string<uint8_t>& sbas, time_t t)
|
|
{
|
|
d_lastSeen = t;
|
|
vector<LongTermCorrection> ret;
|
|
for(int n=0; n < 2; ++n) {
|
|
parse25H(sbas, t, 14 +106 *n, ret);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse24(const basic_string<uint8_t>& sbas, time_t t)
|
|
{
|
|
d_lastSeen = t;
|
|
pair<vector<FastCorrection>, vector<LongTermCorrection>> ret;
|
|
int fcid = getbitu(&sbas[0], 14+98, 2);
|
|
|
|
for(int pos = 0; pos < 6; ++pos) {
|
|
FastCorrection fc;
|
|
int slot = 13*(fcid)+pos+1;
|
|
fc.id = getSBASSatID(slot);
|
|
|
|
fc.correction = getbits(&sbas[0], 14 + 12*pos, 12)*0.125;
|
|
fc.udrei = getbitu(&sbas[0], 14 + 72 + 4*pos, 4);
|
|
fc.lastUpdate = t;
|
|
if(d_slot2prn.count(slot)) {
|
|
|
|
d_fast[fc.id] = fc;
|
|
ret.first.push_back(fc);
|
|
}
|
|
}
|
|
parse25H(sbas, t, 120, ret.second);
|
|
return ret;
|
|
}
|
|
|
|
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> SBASState::parse(const std::basic_string<uint8_t>& sbas, time_t now)
|
|
{
|
|
pair<vector<SBASState::FastCorrection>, vector<SBASState::LongTermCorrection>> ret;
|
|
int type = getbitu(&sbas[0], 8, 6);
|
|
if(type == 0) {
|
|
parse0(sbas, now);
|
|
}
|
|
else if(type == 1) {
|
|
parse1(sbas, now);
|
|
}
|
|
else if(type >= 2 && type <= 5) {
|
|
ret.first = parse2_5(sbas, now);
|
|
}
|
|
|
|
else if(type == 6) {
|
|
ret.first = parse6(sbas, now);
|
|
}
|
|
else if(type ==7) {
|
|
parse7(sbas, now);
|
|
}
|
|
else if(type == 24) {
|
|
ret = parse24(sbas, now);
|
|
}
|
|
else if(type == 25) {
|
|
ret.second = parse25(sbas, now);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void SBASState::parse25H(const basic_string<uint8_t>& sbas, time_t t, int offset, vector<LongTermCorrection>& ret)
|
|
{
|
|
LongTermCorrection ltc;
|
|
ltc.velocity = getbitu(&sbas[0], offset, 1);
|
|
|
|
if(ltc.velocity) { // 1 SV
|
|
int slot = getbitu(&sbas[0], offset + 1, 6);
|
|
ltc.id = getSBASSatID(slot);
|
|
ltc.iod8 = getbitu(&sbas[0], offset + 7, 8);
|
|
|
|
ltc.dx = 0.125*getbits(&sbas[0], offset + 15, 11);
|
|
ltc.dy = 0.125*getbits(&sbas[0], offset + 26, 11);
|
|
ltc.dz = 0.125*getbits(&sbas[0], offset + 37, 11);
|
|
ltc.dai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 48, 11), -31);
|
|
|
|
ltc.ddx = ldexp(getbits(&sbas[0], offset + 59, 8), -11);
|
|
ltc.ddy = ldexp(getbits(&sbas[0], offset + 67, 8), -11);
|
|
ltc.ddz = ldexp(getbits(&sbas[0], offset + 75, 8), -11);
|
|
ltc.ddai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 83, 8), -39);
|
|
|
|
ltc.toa = 16 * getbitu(&sbas[0], offset+91, 13);
|
|
ltc.iodp = getbitu(&sbas[0], offset+104, 2);
|
|
// 105
|
|
ltc.lastUpdate = t;
|
|
if(ltc.toa) {
|
|
ret.push_back(ltc);
|
|
d_longterm[ltc.id]=ltc;
|
|
}
|
|
}
|
|
else {
|
|
for(int n = 0 ; n < 2; ++n) {
|
|
int slot = getbitu(&sbas[0], offset + 1, 6);
|
|
ltc.id = getSBASSatID(slot);
|
|
ltc.iod8 = getbitu(&sbas[0], offset + 7, 8);
|
|
|
|
ltc.dx = 0.125*getbits(&sbas[0], offset + 15, 9);
|
|
ltc.dy = 0.125*getbits(&sbas[0], offset + 24, 9);
|
|
ltc.dz = 0.125*getbits(&sbas[0], offset + 33, 9);
|
|
|
|
ltc.dai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 42, 10), -31);
|
|
|
|
ltc.ddx = ltc.ddy = ltc. ddz = ltc.ddai = 0;
|
|
ltc.lastUpdate = t;
|
|
ret.push_back(ltc);
|
|
d_longterm[ltc.id]=ltc;
|
|
offset += 51;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// old version with ephemeris parsing
|
|
#if 0
|
|
void parseSBAS25H(int sv, const basic_string<uint8_t>& sbas, time_t t, ofstream& sbascsv, int offset, map<int, GPSState>* gpseph, const Point& src)
|
|
{
|
|
bool velocity = getbitu(&sbas[0], offset, 1);
|
|
|
|
if(velocity) { // 1 SV
|
|
int slot = getbitu(&sbas[0], offset + 1, 6);
|
|
int iod = getbitu(&sbas[0], offset + 7, 8);
|
|
|
|
double dx = 0.125*getbits(&sbas[0], offset + 15, 11);
|
|
double dy = 0.125*getbits(&sbas[0], offset + 26, 11);
|
|
double dz = 0.125*getbits(&sbas[0], offset + 37, 11);
|
|
double dai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 48, 11), -31);
|
|
|
|
double ddx = ldexp(getbits(&sbas[0], offset + 53, 8), -11);
|
|
double ddy = ldexp(getbits(&sbas[0], offset + 61, 8), -11);
|
|
double ddz = ldexp(getbits(&sbas[0], offset + 69, 8), -11);
|
|
double ddai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 77, 8), -39);
|
|
|
|
int tvalid = 16 * getbitu(&sbas[0], offset+85, 13);
|
|
if(tvalid) {
|
|
sbascsv << std::fixed<< t <<" " << sv << " ";
|
|
sbascsv << getSBASSV(sv, slot);
|
|
cout << "("<<dx<<", "<<dy<<", "<<dz<<", "<<dai<<") -> ";
|
|
cout << "("<<ddx<<", "<<ddy<<", "<<ddz<<", "<<1000000000*ddai<<") tvalid "<<tvalid<<"\n";
|
|
|
|
|
|
sbascsv <<" " << iod<<" " << dx << " " << dy << " " << dz<<" " << dai;
|
|
sbascsv << " " << ddx <<" " <<ddy <<" " << ddz<<" " << ddai;
|
|
if(gpseph && gpseph->count(getSBASNumber(sv, slot))) {
|
|
auto& gs = (*gpseph)[getSBASNumber(sv, slot)];
|
|
if(gs.isComplete(gs.gpsiod)) {
|
|
Point sat;
|
|
getCoordinates(gs.tow, gs.iods[gs.gpsiod], &sat);
|
|
sbascsv <<" " << sat.x<<" "<<sat.y<<" " << sat.z;
|
|
double prerange = Vector(sat, src).length();
|
|
sat.x += dx; sat.y += dy; sat.z += dz;
|
|
double postrange = Vector(sat, src).length();
|
|
sbascsv<<" " <<postrange - prerange;
|
|
}
|
|
}
|
|
|
|
sbascsv<<"\n";
|
|
}
|
|
}
|
|
else {
|
|
int slot = getbitu(&sbas[0], offset + 1, 6);
|
|
int iod = getbitu(&sbas[0], offset + 7, 8);
|
|
|
|
sbascsv << std::fixed<< t <<" " << sv << " ";
|
|
sbascsv << getSBASSV(sv, slot);
|
|
cout<< getSBASSV(sv, slot);
|
|
cout<<" IOD8 " <<iod<<" ";
|
|
|
|
|
|
double dx = 0.125*getbits(&sbas[0], offset + 15, 9);
|
|
double dy = 0.125*getbits(&sbas[0], offset + 24, 9);
|
|
double dz = 0.125*getbits(&sbas[0], offset + 33, 9);
|
|
double dai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 42, 10), -31);
|
|
cout << "("<<dx<<", "<<dy<<", "<<dz<<", "<< dai<<" ns)\n";
|
|
sbascsv <<" " << iod<<" " << dx << " " << dy << " " << dz<<" " << dai;
|
|
sbascsv <<" 0 0 0 0"; // no delta
|
|
if(gpseph && gpseph->count(getSBASNumber(sv, slot))) {
|
|
auto& gs = (*gpseph)[getSBASNumber(sv, slot)];
|
|
if(gs.isComplete(gs.gpsiod)) {
|
|
Point sat;
|
|
getCoordinates(gs.tow, gs.iods[gs.gpsiod], &sat);
|
|
sbascsv <<" " << sat.x<<" "<<sat.y<<" " << sat.z;
|
|
double prerange = Vector(sat, src).length();
|
|
sat.x += dx; sat.y += dy; sat.z += dz;
|
|
double postrange = Vector(sat, src).length();
|
|
sbascsv<<" " <<postrange - prerange;
|
|
|
|
}
|
|
}
|
|
sbascsv <<'\n';
|
|
|
|
offset += 51;
|
|
|
|
slot = getbitu(&sbas[0], offset + 1, 6);
|
|
iod = getbitu(&sbas[0], offset + 7, 8);
|
|
|
|
sbascsv << t <<" " << sv << " ";
|
|
sbascsv << getSBASSV(sv, slot);
|
|
cout<< getSBASSV(sv, slot);
|
|
cout<<" IOD8 " <<iod<<" ";
|
|
|
|
|
|
dx = 0.125*getbits(&sbas[0], offset + 15, 9);
|
|
dy = 0.125*getbits(&sbas[0], offset + 24, 9);
|
|
dz = 0.125*getbits(&sbas[0], offset + 33, 9);
|
|
dai = 1000000000.0*ldexp(getbits(&sbas[0], offset + 42, 10), -31);
|
|
cout << "("<<dx<<", "<<dy<<", "<<dz<<", "<< dai<<" ns)\n";
|
|
sbascsv << std::fixed <<" " << iod<<" " << dx << " " << dy << " " << dz<<" " << dai <<" 0 0 0 0";
|
|
if(gpseph && gpseph->count(getSBASNumber(sv, slot))) {
|
|
auto& gs = (*gpseph)[getSBASNumber(sv, slot)];
|
|
if(gs.isComplete(gs.gpsiod)) {
|
|
Point sat;
|
|
getCoordinates(gs.tow, gs.iods[gs.gpsiod], &sat);
|
|
sbascsv <<" " << sat.x<<" "<<sat.y<<" " << sat.z;
|
|
double prerange = Vector(sat, src).length();
|
|
sat.x += dx; sat.y += dy; sat.z += dz;
|
|
double postrange = Vector(sat, src).length();
|
|
sbascsv<<" " <<postrange - prerange;
|
|
}
|
|
}
|
|
sbascsv <<'\n';
|
|
}
|
|
}
|
|
#endif
|