2019-08-12 07:54:16 -06:00
|
|
|
#include "gps.hh"
|
|
|
|
|
|
|
|
// this strips out spare bits + parity, and leaves 10 clean 24 bit words
|
|
|
|
std::basic_string<uint8_t> getCondensedGPSMessage(std::basic_string_view<uint8_t> payload)
|
|
|
|
{
|
|
|
|
uint8_t buffer[10*24/8];
|
|
|
|
|
|
|
|
for(int w = 0 ; w < 10; ++w) {
|
|
|
|
setbitu(buffer, 24*w, 24, getbitu(&payload[0], 2 + w*32, 24));
|
|
|
|
}
|
|
|
|
|
|
|
|
return std::basic_string<uint8_t>(buffer, 30);
|
|
|
|
|
|
|
|
}
|
|
|
|
|
2020-02-25 04:12:35 -07:00
|
|
|
// 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);
|
2021-07-24 12:57:01 -06:00
|
|
|
|
2020-02-25 04:12:35 -07:00
|
|
|
dtLS = getbits(&cond[0], 8*24, 8);
|
2021-07-24 12:57:01 -06:00
|
|
|
wnLSF= getbitu(&cond[0], 8*24 + 8, 8);
|
|
|
|
dn = getbitu(&cond[0], 8*24 + 16, 8);
|
2020-02-25 04:12:35 -07:00
|
|
|
dtLSF = getbits(&cond[0], 9*24, 8);
|
2021-07-24 12:57:01 -06:00
|
|
|
|
2020-02-25 04:12:35 -07:00
|
|
|
|
|
|
|
// 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;
|
|
|
|
}
|